From ae34a68d987074c3795eef454d52d95f5cc71485 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Fri, 14 Jul 2023 17:40:11 +0200
Subject: [PATCH] embObjIMU: fixes for ems

---
 src/libraries/icubmod/embObjIMU/embObjIMU.cpp       | 2 +-
 src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp | 6 +++---
 src/libraries/icubmod/embObjLib/serviceParser.cpp   | 7 ++++---
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/libraries/icubmod/embObjIMU/embObjIMU.cpp b/src/libraries/icubmod/embObjIMU/embObjIMU.cpp
index a7e9ca4f7b..c7dfe0cef7 100644
--- a/src/libraries/icubmod/embObjIMU/embObjIMU.cpp
+++ b/src/libraries/icubmod/embObjIMU/embObjIMU.cpp
@@ -101,7 +101,7 @@ bool embObjIMU::open(yarp::os::Searchable &config)
     for (int i=0; i<servCfg.id.size(); i++) {
         auto type = static_cast<eOas_sensor_t>(servCfg.inertials[i].typeofsensor);
         if ((eoas_accel_mtb_int == type) || (eoas_accel_mtb_ext == type) || (eoas_gyros_mtb_ext == type) ||
-            (eoas_gyros_st_l3g4200d == type) || (eoas_accel_st_lis3x == type)) {
+            (eoas_gyros_st_l3g4200d == type)) {
             servCfg.convFactors.accFactor = 1.0; // For now let's keep 1.0
             servCfg.convFactors.magFactor = 1.0;
             servCfg.convFactors.gyrFactor = 1.0;
diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
index 25327ea1a9..4cc20ba931 100644
--- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
+++ b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
@@ -73,7 +73,7 @@ bool PositionMaps::getIndex(const eOas_inertial3_data_t* data, uint8_t& index, e
     }
     type = static_cast<eOas_sensor_t>(data->typeofsensor);
 
-    if(type == eoas_gyros_st_l3g4200d || eoas_accel_st_lis3x == type) {
+    if(type == eoas_gyros_st_l3g4200d) {
         index = ethpositionmap[data->typeofsensor];
     }
     else {
@@ -86,7 +86,7 @@ bool PositionMaps::getIndex(const eOas_inertial3_data_t* data, uint8_t& index, e
 bool PositionMaps::getIndex(eOas_sensor_t type, uint8_t canbus, uint8_t canaddress, uint8_t& index)
 {
     // It is the ems, it is an eth board
-    if(type == eoas_gyros_st_l3g4200d || eoas_accel_st_lis3x == type) {
+    if(type == eoas_gyros_st_l3g4200d) {
         return false;
     }
     if(canbus >= eOcanports_number)
@@ -108,7 +108,7 @@ bool PositionMaps::getCanAddress(const eOas_inertial3_data_t *data, uint8_t &can
         return false;
     auto type = static_cast<eOas_sensor_t>(data->typeofsensor);
     // It is the ems, it is an eth board
-    if (eoas_gyros_st_l3g4200d == type || eoas_accel_st_lis3x == type) {
+    if (eoas_gyros_st_l3g4200d == type) {
         return false;
     }
     canbus = data->id >> 4;
diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp
index 4ecded0b1c..8cfb148b42 100755
--- a/src/libraries/icubmod/embObjLib/serviceParser.cpp
+++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp
@@ -1700,7 +1700,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
         if( (eoas_imu_acc != type) && (eoas_imu_mag != type) && (eoas_imu_gyr != type) && (eoas_imu_eul != type) &&
             (eoas_imu_qua != type) && (eoas_imu_lia != type) && (eoas_imu_grv != type) && (eoas_imu_status != type) &&
             (eoas_accel_mtb_int != type) && (eoas_accel_mtb_ext != type) && (eoas_gyros_mtb_ext != type) &&
-            (eoas_gyros_st_l3g4200d != type) && (eoas_accel_st_lis3x != type))
+            (eoas_gyros_st_l3g4200d != type))
         {
             yWarning() << "ServiceParser::parseService() has detected a wrong inertial sensor:" << eoas_sensor2string(type) << " ...  we drop it";
             continue;
@@ -1712,7 +1712,8 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
         memcpy(&des.on, &sensor.location, sizeof(eObrd_location_t));
 
         const eObrd_info_t *boardInfo_ptr =  eoas_inertial3_setof_boardinfos_find(boardInfoSet_ptr, eoboards_type2cantype(sensor.boardtype));
-        if(nullptr == boardInfo_ptr)//if I did not already insert the borad info with type == sensor.boardtype, now I insert it
+        if(nullptr == boardInfo_ptr && sensor.boardtype != eobrd_ems4) // If the sensor is an ems, it does not make sense to find it in can boards
+        //if I did not already insert the borad info with type == sensor.boardtype, now I insert it
         {
             // first of all I need to find the board info for this board type
             bool found = false;
@@ -1727,7 +1728,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
             }
             if(!found)
             {
-                yError() << "ServiceParser::parseService(IMU). The sensor " << i << "with type "<<  eoas_sensor2string(static_cast<eOas_sensor_t> (des.typeofsensor)) << "has borad type  " << eoboards_type2string2(sensor.boardtype, false) << " that is not declared in the SERVICE.PROPERTIES.CANBOARDS tag";
+                yError() << "ServiceParser::parseService(IMU). The sensor " << sensor.boardtype << "with type "<<  eoas_sensor2string(static_cast<eOas_sensor_t> (des.typeofsensor)) << "has borad type  " << eoboards_type2string2(sensor.boardtype, false) << " that is not declared in the SERVICE.PROPERTIES.CANBOARDS tag";
                 return false;
             }
             eObrd_info_t boardInfo = {};