From b7ad61c1dcf6ac80101be8b324568c89e65737a5 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 14 Nov 2023 18:35:15 +0100 Subject: [PATCH] embObjIMU: refactor in order to remove imuMeasureConverter --- .../icubmod/embObjIMU/CMakeLists.txt | 2 +- src/libraries/icubmod/embObjIMU/embObjIMU.cpp | 18 ------ .../icubmod/embObjIMU/eo_imu_privData.cpp | 48 +++++----------- .../icubmod/embObjIMU/eo_imu_privData.h | 5 +- .../icubmod/embObjIMU/imuMeasureConverter.cpp | 55 ------------------- .../icubmod/embObjIMU/imuMeasureConverter.h | 37 ------------- 6 files changed, 18 insertions(+), 147 deletions(-) delete mode 100644 src/libraries/icubmod/embObjIMU/imuMeasureConverter.cpp delete mode 100644 src/libraries/icubmod/embObjIMU/imuMeasureConverter.h diff --git a/src/libraries/icubmod/embObjIMU/CMakeLists.txt b/src/libraries/icubmod/embObjIMU/CMakeLists.txt index 427c17babc..0508fee9c4 100644 --- a/src/libraries/icubmod/embObjIMU/CMakeLists.txt +++ b/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) diff --git a/src/libraries/icubmod/embObjIMU/embObjIMU.cpp b/src/libraries/icubmod/embObjIMU/embObjIMU.cpp index e8607be7d4..b652ee9c9d 100644 --- a/src/libraries/icubmod/embObjIMU/embObjIMU.cpp +++ b/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.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)) diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp b/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp index 81b8eedd5a..2affcf9808 100644 --- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp +++ b/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 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; ivalues[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; diff --git a/src/libraries/icubmod/embObjIMU/eo_imu_privData.h b/src/libraries/icubmod/embObjIMU/eo_imu_privData.h index fb0d340b2a..ee6096ef42 100644 --- a/src/libraries/icubmod/embObjIMU/eo_imu_privData.h +++ b/src/libraries/icubmod/embObjIMU/eo_imu_privData.h @@ -10,9 +10,9 @@ #define __eo_imu_privData_h__ #include "embObjGeneralDevPrivData.h" -#include "imuMeasureConverter.h" #include #include +#include #include @@ -42,7 +42,6 @@ class yarp::dev::PositionMaps // data used for handling the received messsages }; - typedef struct { std::string name; @@ -50,6 +49,7 @@ typedef struct 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 measConverters; SensorsData(); void init(servConfigImu_t &servCfg, string error_string); bool update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_t *newdata); diff --git a/src/libraries/icubmod/embObjIMU/imuMeasureConverter.cpp b/src/libraries/icubmod/embObjIMU/imuMeasureConverter.cpp deleted file mode 100644 index 31eb312a16..0000000000 --- a/src/libraries/icubmod/embObjIMU/imuMeasureConverter.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * Author Valentina Gaggero - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include "imuMeasureConverter.h" - -// double accFactor; //raw to metric measure -// double gyrFactor; //raw to metric measure -// double magFactor; //raw to metric measure - -ImuMeasureConverter::ImuMeasureConverter() -{ - double accFactor = 1.0; - double gyrFactor = 1.0; - double magFactor = 1.0; - double eulFactor = 1.0; -} - -ImuMeasureConverter::ImuMeasureConverter(double accConvFactor, double gyrConvFactor, double magConvFactor, double eulConvFactor) -{ - Initialize(accConvFactor, gyrConvFactor, magConvFactor, eulConvFactor); -} - -void ImuMeasureConverter::Initialize(double accConvFactor, double gyrConvFactor, double magConvFactor, double eulConvFactor) -{ - accFactor = accConvFactor; - gyrFactor = gyrConvFactor; - magFactor = magConvFactor; - eulFactor = eulConvFactor; -} - -double ImuMeasureConverter::convertAcc_raw2metric(double accRaw) const -{ - return accRaw / accFactor; -} -double ImuMeasureConverter::convertGyr_raw2metric(double gyrRaw) const -{ - return gyrRaw / gyrFactor; -} -double ImuMeasureConverter::convertMag_raw2metric(double magRaw) const -{ - return magRaw / magFactor; -} - -double ImuMeasureConverter::convertEul_raw2metric(double eulRaw) const -{ - return eulRaw/eulFactor; -} - diff --git a/src/libraries/icubmod/embObjIMU/imuMeasureConverter.h b/src/libraries/icubmod/embObjIMU/imuMeasureConverter.h deleted file mode 100644 index b66d5baeaa..0000000000 --- a/src/libraries/icubmod/embObjIMU/imuMeasureConverter.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * Author Valentina Gaggero - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef __ImuMeasureConverter_h__ -#define __ImuMeasureConverter_h__ - -// namespace yarp { -// namespace dev { -// class ImuMeasureConverter; -// } -// } -class ImuMeasureConverter -{ -private: - double accFactor; //raw to metric measure - double gyrFactor; //raw to metric measure - double magFactor; //raw to metric measure - double eulFactor; //raw to metric measure -public: - ImuMeasureConverter(); - ImuMeasureConverter(double accConvFactor, double gyrConvFactor, double magConvFactor, double eulConvFactor); - void Initialize(double accConvFactor, double gyrConvFactor, double magConvFactor, double eulConvFactor); - - double convertAcc_raw2metric(double) const; - double convertGyr_raw2metric(double) const; - double convertMag_raw2metric(double) const; - double convertEul_raw2metric(double) const; -}; - -#endif \ No newline at end of file