embObjIMU: refactor in order to remove imuMeasureConverter

src/libraries/icubmod/embObjIMU/CMakeLists.txt

@@ -15,7 +15,7 @@ IF (NOT SKIP_embObjIMU)
 #   message(INFO " embObjIMU - embObj_includes: ${embObj_includes}, ${CMAKE_CURRENT_SOURCE_DIR}/")
   INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
 
-  yarp_add_plugin(embObjIMU embObjIMU.cpp embObjIMU.h eo_imu_privData.h  eo_imu_privData.cpp imuMeasureConverter.cpp imuMeasureConverter.h )
+  yarp_add_plugin(embObjIMU embObjIMU.cpp embObjIMU.h eo_imu_privData.h  eo_imu_privData.cpp)
   TARGET_LINK_LIBRARIES(embObjIMU ethResources)
   icub_export_plugin(embObjIMU)
 

src/libraries/icubmod/embObjIMU/embObjIMU.cpp

@@ -144,24 +144,6 @@ bool embObjIMU::open(yarp::os::Searchable &config)
     //init conversion factor
     //TODO: currently the conversion factors are not read from xml files, but configured here.
     //please read IMUbosh datasheet for more information
-    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)) {
-            servCfg.convFactors.accFactor = 1.0; // For now let's keep 1.0
-            servCfg.convFactors.magFactor = 1.0;
-            servCfg.convFactors.gyrFactor = 1.0;
-            servCfg.convFactors.eulFactor = 1.0;
-        }
-        else {
-            servCfg.convFactors.accFactor = 100.0; // 1 m/sec2 = 100 binary units
-            servCfg.convFactors.magFactor = 16.0 * 1000000.0;  // 1 microT = 16 binary units
-            servCfg.convFactors.gyrFactor = 16.0;  // 1 degree/sec = 16 binary units
-            servCfg.convFactors.eulFactor = 16.0; //  1 degree  = 16 binary units
-            //eul angles don't need a conversion.
-        }
-        GET_privData(mPriv).sens.measConverters.push_back({servCfg.convFactors.accFactor, servCfg.convFactors.gyrFactor,  servCfg.convFactors.magFactor, servCfg.convFactors.eulFactor});
-    }
     // configure the sensor(s)
 
     if(false == GET_privData(mPriv).sendConfing2board(servCfg))

src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp

@@ -157,6 +157,18 @@ void SensorsData::init(servConfigImu_t &servCfg, string error_string)
                     newSensor.name = servCfg.sensorName[i];
                 }
                 newSensor.framename = newSensor.name;
+                switch (des->typeofsensor) {
+					case eoas_imu_acc:
+						newSensor.conversionFactor = 100.0; // 1 m/sec2 = 100 binary units
+						break;
+                    case eoas_imu_mag:
+                        newSensor.conversionFactor = 16.0 * 1000000.0; // 1 microT = 16 binary units
+						break;
+                    case eoas_imu_gyr:
+						newSensor.conversionFactor = 16.0; // 1 degree/sec = 16 binary units
+                    case eoas_imu_eul:
+                        newSensor.conversionFactor = 16.0; // 1 degree = 16 binary units
+				}
                 if(des->typeofsensor == eoas_imu_qua)
                     newSensor.values.resize(4);
                 else
@@ -230,36 +242,6 @@ bool SensorsData::getSensorMeasure(size_t sens_index, eOas_sensor_t type, yarp::
     try
     {   std::lock_guard<std::mutex> lck (mutex);
         out = mysens[type].at(sens_index).values;
-        switch(type)
-        {
-            case eoas_imu_acc:
-            case eoas_accel_mtb_int:
-            case eoas_accel_mtb_ext:    
-            {
-                for(int i=0; i<out.size(); i++)
-                    out[i] = measConverters[sens_index].convertAcc_raw2metric(out[i]);
-            }break;
-
-            case  eoas_imu_mag:
-            {    for(int i=0; i<out.size(); i++)
-                out[i] = measConverters[sens_index].convertMag_raw2metric(out[i]);
-            }break;
-
-            case eoas_imu_gyr:
-            case eoas_gyros_st_l3g4200d:
-            case eoas_gyros_mtb_ext:
-            {    for(int i=0; i<out.size(); i++)
-                out[i] = measConverters[sens_index].convertGyr_raw2metric(out[i]);
-            }break;
-
-            case eoas_imu_eul:
-            {
-                for(int i=0; i<out.size(); i++)
-                    out[i] = measConverters[sens_index].convertEul_raw2metric(out[i]);
-            }
-
-            default: break;
-        };
         timestamp = mysens[type].at(sens_index).timestamp;
     }
     catch (const std::out_of_range& oor)
@@ -276,9 +258,9 @@ bool SensorsData::update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_
 
     sensorInfo_t *info = &(mysens[type][index]);
 
-    info->values[0] = newdata->x;
-    info->values[1] = newdata->y;
-    info->values[2] = newdata->z;
+    info->values[0] = newdata->x / info->conversionFactor;
+    info->values[1] = newdata->y / info->conversionFactor;
+    info->values[2] = newdata->z / info->conversionFactor;
     info->timestamp = yarp::os::Time::now();
 
     return true;

src/libraries/icubmod/embObjIMU/eo_imu_privData.h

@@ -10,9 +10,9 @@
 #define __eo_imu_privData_h__
 
 #include "embObjGeneralDevPrivData.h"
-#include "imuMeasureConverter.h"
 #include <yarp/sig/Vector.h>
 #include <mutex>
+#include <map>
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 
 
@@ -42,14 +42,14 @@ class yarp::dev::PositionMaps // data used for handling the received messsages
 };
 
 
-
 typedef struct
 {
     std::string name;
     std::string framename;
     yarp::sig::Vector values;
     uint8_t state;
     double timestamp;
+    double conversionFactor{1.0};  // raw to metric measure
 } sensorInfo_t;
 
 class yarp::dev::SensorsData
@@ -60,7 +60,6 @@ class yarp::dev::SensorsData
     string errorstring;
 
 public:
-    std::vector<ImuMeasureConverter> measConverters;
     SensorsData();
     void init(servConfigImu_t &servCfg, string error_string);
     bool update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_t *newdata);