Skip to content

Commit

Permalink
embObjIMU: refactor in order to remove imuMeasureConverter
Browse files Browse the repository at this point in the history
  • Loading branch information
Nicogene committed Nov 23, 2023
1 parent e910b9e commit 512c019
Show file tree
Hide file tree
Showing 6 changed files with 18 additions and 147 deletions.
2 changes: 1 addition & 1 deletion src/libraries/icubmod/embObjIMU/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
18 changes: 0 additions & 18 deletions src/libraries/icubmod/embObjIMU/embObjIMU.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
48 changes: 15 additions & 33 deletions src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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;
Expand Down
5 changes: 2 additions & 3 deletions src/libraries/icubmod/embObjIMU/eo_imu_privData.h
Original file line number Diff line number Diff line change
Expand Up @@ -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>


Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down
55 changes: 0 additions & 55 deletions src/libraries/icubmod/embObjIMU/imuMeasureConverter.cpp

This file was deleted.

37 changes: 0 additions & 37 deletions src/libraries/icubmod/embObjIMU/imuMeasureConverter.h

This file was deleted.

0 comments on commit 512c019

Please sign in to comment.