Changeset 343 in flair-src for trunk/lib/FlairSensorActuator/src/Imu.cpp

Timestamp:

Dec 12, 2019, 6:11:36 PM (5 years ago)

Author:

Sanahuja Guillaume

Message:

update imu

File:

• trunk/lib/FlairSensorActuator/src/Imu.cpp (modified) (2 diffs)

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -35 +35 @@
 Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) {
   imuData = new ImuData(this);
+  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
@@ -82 +91 @@
 }
 
+void Imu::RemoveOffsets(Vector3Df& acc,Vector3Df& gyr) {
+  Time time=GetTime();  
+  
+  if(startCalcOffset==0) {
+    startCalcOffset=time;
+    Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
+  } 
+  
+  if(!calibrationDone) {
+    if(time<startCalcOffset+(Time)10000000000) {
+      accOffset+=acc;
+      gyrOffset+=gyr;
+      cptOffset++;
+    } else {
+      Printf("%s calibration done\n",ObjectName().c_str());
+      SetIsReady(true);
+      calibrationDone=true;
+    } 
+    acc.x=0;
+    acc.y=0;
+    acc.z=0;
+    gyr.x=0;
+    gyr.y=0;
+    gyr.z=0;
+  } else { //calibrationDone
+     acc-=accOffset/cptOffset;
+     gyr-=gyrOffset/cptOffset;
+  }
+  
+}
+
+
 GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }