strain2: fix setting of orientation at startup (#390)

robotology · Jun 16, 2023 · 9bc2c4d · 9bc2c4d
2 parents 25728f0 + fa46cd0
commit 9bc2c4d
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 16 deletions.
diff --git a/emBODY/eBcode/arch-arm/board/strain2/application/src/main-strain2-application.cpp b/emBODY/eBcode/arch-arm/board/strain2/application/src/main-strain2-application.cpp
@@ -13,7 +13,7 @@
 
 constexpr embot::app::theCANboardInfo::applicationInfo applInfo
 {
-    embot::prot::can::versionOfAPPLICATION {2, 1, 0},
+    embot::prot::can::versionOfAPPLICATION {2, 2, 0},
     embot::prot::can::versionOfCANPROTOCOL {2, 0}
 };
 

diff --git a/emBODY/eBcode/arch-arm/board/strain2c/application/src/main-strain2-application.cpp b/emBODY/eBcode/arch-arm/board/strain2c/application/src/main-strain2-application.cpp
@@ -13,7 +13,7 @@
 
 constexpr embot::app::theCANboardInfo::applicationInfo applInfo
 {
-    embot::prot::can::versionOfAPPLICATION {2, 1, 0},
+    embot::prot::can::versionOfAPPLICATION {2, 2, 0},
     embot::prot::can::versionOfCANPROTOCOL {2, 0}
 };
 

diff --git a/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theIMU.cpp b/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theIMU.cpp
@@ -123,6 +123,19 @@ struct embot::app::application::theIMU::Impl
 
     Config config;
 
+    static constexpr embot::hw::bno055::Placement defaultboardplacement 
+    {
+    // remapping of  the axis to have IMU and FT frames parallel, neglecting non-inertial corrections, see this github issue:
+    // https://github.com/icub-tech-iit/fix/issues/780
+    //this macro defines different behaviour for the IMU placed in the STRAIN2 board, i.e. the accelerometer/gyroscope/magnetometer axes are remapped to match the FT frame
+    // In strain2 we have the P6 configuration reported in bno055 datasheet
+#if defined(STM32HAL_BOARD_STRAIN2) || (STM32HAL_BOARD_STRAIN2C)
+        embot::hw::bno055::Placement::P6
+#else
+        embot::hw::bno055::Placement::P1
+#endif
+    };
+
     bool ticking;
 
     embot::os::Timer *ticktimer;
@@ -360,7 +373,7 @@ bool embot::app::application::theIMU::Impl::processdata(std::vector<embot::prot:
         {
             // generate a acc message with canrevisitedconfig.imuinfo.counter
             info.sensor = embot::prot::can::analog::imuSensor::acc;
-	        info.value = imuacquisition.data.acc;
+            info.value = imuacquisition.data.acc;
 
             msg.load(info);
             msg.get(frame);
@@ -520,17 +533,10 @@ bool embot::app::application::theIMU::initialise(Config &config)
     pImpl->action.load(embot::os::EventToThread(pImpl->config.tickevent, pImpl->config.totask));
 
     embot::hw::bno055::init(pImpl->config.sensor, pImpl->config.sensorconfig);
-    // remapping of  the axis to have IMU and FT frames parallel, neglecting non-inertial corrections, see this github issue:
-    // https://github.com/icub-tech-iit/fix/issues/780
-    //this macro defines different behaviour for the IMU placed in the STRAIN2 board, i.e. the accelerometer/gyroscope/magnetometer axes are remapped to match the FT frame
-#if defined(STM32HAL_BOARD_STRAIN2)
-    // In strain2 we have the P6 configuration reported in bno055 datasheet
-    embot::hw::bno055::write(pImpl->config.sensor, embot::hw::bno055::Register::AXIS_MAP_CONFIG, 0x21, 5*embot::core::time1millisec);
-    embot::hw::bno055::write(pImpl->config.sensor, embot::hw::bno055::Register::AXIS_MAP_SIGN, 0x07, 5*embot::core::time1millisec);
-#endif
-    embot::hw::bno055::set(pImpl->config.sensor, embot::hw::bno055::Mode::NDOF, 5*embot::core::time1millisec);
 
+    embot::hw::result_t res = embot::hw::bno055::set(pImpl->config.sensor, pImpl->defaultboardplacement, 5*embot::core::time1millisec);
 
+    embot::hw::bno055::set(pImpl->config.sensor, embot::hw::bno055::Mode::NDOF, 5*embot::core::time1millisec);
 
     return true;
 }
@@ -623,26 +629,26 @@ bool embot::app::application::theIMU::set(const embot::prot::can::analog::pollin
     embot::hw::bno055::get(pImpl->config.sensor, placement, 5*embot::core::time1millisec);    
 
     // now we transform info.orientation into a  ... target placement for the bno055
-    embot::hw::bno055::Placement targetplacement {embot::hw::bno055::Placement::P1};
+    embot::hw::bno055::Placement targetplacement {pImpl->defaultboardplacement};
     if(embot::prot::can::analog::polling::IMUorientation::TYPE::factorydefault == info.orientation.type) 
     {   // we use the placement used by the bno055 at boostrap
-        targetplacement = embot::hw::bno055::Placement::P1;
+        targetplacement = pImpl->defaultboardplacement;
     }
     else if(true == embot::hw::bno055::isvalidplacement(info.orientation.param))
     {   // we have a placement which someone wants to impose. and it is valid
         targetplacement = embot::hw::bno055::toplacement(info.orientation.param);
     }
     else
     {   // the placemnet someone wants to impose is not valid. i revert to default one
-        targetplacement = embot::hw::bno055::Placement::P1;
+        targetplacement = pImpl->defaultboardplacement;
     }
 
 
     bool changebno055 = (placement != targetplacement) || (mode != targetmode); 
 
     if(changebno055)
     {
-        // from Table 3-6: Operating mode swithcing time
+        // from Table 3-6: Operating mode switching time
         constexpr embot::core::relTime waitANY2CONFIG {19 * embot::core::time1millisec};
         constexpr embot::core::relTime waitCONFIG2ANY {7 * embot::core::time1millisec};