Changeset 344 in flair-src for trunk/lib/FlairSensorActuator

Timestamp:

Dec 20, 2019, 5:26:59 PM (5 years ago)

Author:

Sanahuja Guillaume

Message:

improve imu calibration by detecting a movement

Location:

trunk/lib/FlairSensorActuator/src

Files:

• Imu.cpp (modified) (3 diffs)
• Imu.h (modified) (1 diff)

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -37 +37 @@
   startCalcOffset=0;
   calibrationDone=false;
-  accOffset.x=0;
-  accOffset.y=0;
-  accOffset.z=0;
-  gyrOffset.x=0;
-  gyrOffset.y=0;
-  gyrOffset.z=0;
-  cptOffset=0;
 
   // station sol
@@ -96 +89 @@
   if(startCalcOffset==0) {
     startCalcOffset=time;
+    accOffset.x=0;
+    accOffset.y=0;
+    accOffset.z=0;
+    gyrOffset.x=0;
+    gyrOffset.y=0;
+    gyrOffset.z=0;
+    cptOffset=0;
+    accMin=acc;
+    accMax=acc;
     Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
   } 
   
   if(!calibrationDone) {
-    if(time<startCalcOffset+(Time)10000000000) {
+    if(time<startCalcOffset+(Time)10000000000) {//calibrating
       accOffset+=acc;
       gyrOffset+=gyr;
       cptOffset++;
-    } else {
-      Printf("%s calibration done\n",ObjectName().c_str());
-      SetIsReady(true);
-      calibrationDone=true;
+      if(acc.GetNorm()<accMin.GetNorm()) accMin=acc;
+      if(acc.GetNorm()>accMax.GetNorm()) accMax=acc;
+    } else {//check if imu moved or not
+      if((accMax-accMin).GetNorm()>0.5) {//put a parameter
+         Printf("%s imu was moved, calibrating again!\n",ObjectName().c_str());
+         startCalcOffset=0;
+      } else {
+        Printf("%s calibration done\n",ObjectName().c_str());
+        SetIsReady(true);
+        accOffset=accOffset/cptOffset;
+        gyrOffset=gyrOffset/cptOffset;
+        calibrationDone=true;
+      }
     } 
     acc.x=0;
@@ -116 +127 @@
     gyr.z=0;
   } else { //calibrationDone
-     acc-=accOffset/cptOffset;
-     gyr-=gyrOffset/cptOffset;
+     acc-=accOffset;
+     gyr-=gyrOffset;
   }
   

trunk/lib/FlairSensorActuator/src/Imu.h

@@ -170 +170 @@
   gui::DataPlot1D *mxPlot, *myPlot, *mzPlot;
   
+  //for calibration:
   core::Time startCalcOffset;
   bool calibrationDone;
-  core::Vector3Df accOffset,gyrOffset;
+  core::Vector3Df accOffset,gyrOffset,accMax,accMin;
   uint16_t cptOffset;