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

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

update imu

Location:

trunk/lib/FlairSensorActuator/src

Files:

• Imu.cpp (modified) (2 diffs)
• Imu.h (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; }
 

trunk/lib/FlairSensorActuator/src/Imu.h

@@ -147 +147 @@
   */
   void GetDatas(core::ImuData **imuData) const;
+  
+  /*!
+  * \brief Remove Offsets
+  *
+  * Remove the accelerometers and gyrometers offsets. During first 10s, returns 0 (gathering data to get offset)
+  * Then, returns the vectors without offset
+  *
+  * \param acc accelerometer vector to remove offset
+  * \param gyr gyrometer vector to remove offset
+  */
+  void RemoveOffsets(core::Vector3Df& acc,core::Vector3Df& gyr);
 
 private:
@@ -158 +169 @@
   gui::DataPlot1D *gxPlot, *gyPlot, *gzPlot;
   gui::DataPlot1D *mxPlot, *myPlot, *mzPlot;
+  
+  core::Time startCalcOffset;
+  bool calibrationDone;
+  core::Vector3Df accOffset,gyrOffset;
+  uint16_t cptOffset;
+  
 };
 } // end namespace sensor