Changeset 344 in flair-src


Ignore:
Timestamp:
Dec 20, 2019, 5:26:59 PM (2 years ago)
Author:
Sanahuja Guillaume
Message:

improve imu calibration by detecting a movement

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

Legend:

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

    r343 r344  
    3737  startCalcOffset=0;
    3838  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;
    4639
    4740  // station sol
     
    9689  if(startCalcOffset==0) {
    9790    startCalcOffset=time;
     91    accOffset.x=0;
     92    accOffset.y=0;
     93    accOffset.z=0;
     94    gyrOffset.x=0;
     95    gyrOffset.y=0;
     96    gyrOffset.z=0;
     97    cptOffset=0;
     98    accMin=acc;
     99    accMax=acc;
    98100    Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str());
    99101  }
    100102 
    101103  if(!calibrationDone) {
    102     if(time<startCalcOffset+(Time)10000000000) {
     104    if(time<startCalcOffset+(Time)10000000000) {//calibrating
    103105      accOffset+=acc;
    104106      gyrOffset+=gyr;
    105107      cptOffset++;
    106     } else {
    107       Printf("%s calibration done\n",ObjectName().c_str());
    108       SetIsReady(true);
    109       calibrationDone=true;
     108      if(acc.GetNorm()<accMin.GetNorm()) accMin=acc;
     109      if(acc.GetNorm()>accMax.GetNorm()) accMax=acc;
     110    } else {//check if imu moved or not
     111      if((accMax-accMin).GetNorm()>0.5) {//put a parameter
     112         Printf("%s imu was moved, calibrating again!\n",ObjectName().c_str());
     113         startCalcOffset=0;
     114      } else {
     115        Printf("%s calibration done\n",ObjectName().c_str());
     116        SetIsReady(true);
     117        accOffset=accOffset/cptOffset;
     118        gyrOffset=gyrOffset/cptOffset;
     119        calibrationDone=true;
     120      }
    110121    }
    111122    acc.x=0;
     
    116127    gyr.z=0;
    117128  } else { //calibrationDone
    118      acc-=accOffset/cptOffset;
    119      gyr-=gyrOffset/cptOffset;
     129     acc-=accOffset;
     130     gyr-=gyrOffset;
    120131  }
    121132 
  • trunk/lib/FlairSensorActuator/src/Imu.h

    r343 r344  
    170170  gui::DataPlot1D *mxPlot, *myPlot, *mzPlot;
    171171 
     172  //for calibration:
    172173  core::Time startCalcOffset;
    173174  bool calibrationDone;
    174   core::Vector3Df accOffset,gyrOffset;
     175  core::Vector3Df accOffset,gyrOffset,accMax,accMin;
    175176  uint16_t cptOffset;
    176177 
Note: See TracChangeset for help on using the changeset viewer.