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


Ignore:
Timestamp:
12/12/19 18:11:36 (2 years ago)
Author:
Sanahuja Guillaume
Message:

update imu

File:
1 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
Note: See TracChangeset for help on using the changeset viewer.