Changeset 343 in flair-src


Ignore:
Timestamp:
Dec 12, 2019, 6:11:36 PM (5 years ago)
Author:
Sanahuja Guillaume
Message:

update imu

Location:
trunk/lib/FlairSensorActuator/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/Imu.cpp

    r340 r343  
    3535Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) {
    3636  imuData = new ImuData(this);
     37  startCalcOffset=0;
     38  calibrationDone=false;
     39  accOffset.x=0;
     40  accOffset.y=0;
     41  accOffset.z=0;
     42  gyrOffset.x=0;
     43  gyrOffset.y=0;
     44  gyrOffset.z=0;
     45  cptOffset=0;
    3746
    3847  // station sol
     
    8291}
    8392
     93void Imu::RemoveOffsets(Vector3Df& acc,Vector3Df& gyr) {
     94  Time time=GetTime(); 
     95 
     96  if(startCalcOffset==0) {
     97    startCalcOffset=time;
     98    Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
     99  }
     100 
     101  if(!calibrationDone) {
     102    if(time<startCalcOffset+(Time)10000000000) {
     103      accOffset+=acc;
     104      gyrOffset+=gyr;
     105      cptOffset++;
     106    } else {
     107      Printf("%s calibration done\n",ObjectName().c_str());
     108      SetIsReady(true);
     109      calibrationDone=true;
     110    }
     111    acc.x=0;
     112    acc.y=0;
     113    acc.z=0;
     114    gyr.x=0;
     115    gyr.y=0;
     116    gyr.z=0;
     117  } else { //calibrationDone
     118     acc-=accOffset/cptOffset;
     119     gyr-=gyrOffset/cptOffset;
     120  }
     121 
     122}
     123
     124
    84125GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; }
    85126
  • trunk/lib/FlairSensorActuator/src/Imu.h

    r340 r343  
    147147  */
    148148  void GetDatas(core::ImuData **imuData) const;
     149 
     150  /*!
     151  * \brief Remove Offsets
     152  *
     153  * Remove the accelerometers and gyrometers offsets. During first 10s, returns 0 (gathering data to get offset)
     154  * Then, returns the vectors without offset
     155  *
     156  * \param acc accelerometer vector to remove offset
     157  * \param gyr gyrometer vector to remove offset
     158  */
     159  void RemoveOffsets(core::Vector3Df& acc,core::Vector3Df& gyr);
    149160
    150161private:
     
    158169  gui::DataPlot1D *gxPlot, *gyPlot, *gzPlot;
    159170  gui::DataPlot1D *mxPlot, *myPlot, *mzPlot;
     171 
     172  core::Time startCalcOffset;
     173  bool calibrationDone;
     174  core::Vector3Df accOffset,gyrOffset;
     175  uint16_t cptOffset;
     176 
    160177};
    161178} // end namespace sensor
Note: See TracChangeset for help on using the changeset viewer.