- Timestamp:
- Dec 20, 2019, 5:26:59 PM (5 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Imu.cpp
r343 r344 37 37 startCalcOffset=0; 38 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;46 39 47 40 // station sol … … 96 89 if(startCalcOffset==0) { 97 90 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; 98 100 Printf("%s calibrating offsets, do not move imu during 10s\n",ObjectName().c_str()); 99 101 } 100 102 101 103 if(!calibrationDone) { 102 if(time<startCalcOffset+(Time)10000000000) { 104 if(time<startCalcOffset+(Time)10000000000) {//calibrating 103 105 accOffset+=acc; 104 106 gyrOffset+=gyr; 105 107 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 } 110 121 } 111 122 acc.x=0; … … 116 127 gyr.z=0; 117 128 } else { //calibrationDone 118 acc-=accOffset /cptOffset;119 gyr-=gyrOffset /cptOffset;129 acc-=accOffset; 130 gyr-=gyrOffset; 120 131 } 121 132 -
trunk/lib/FlairSensorActuator/src/Imu.h
r343 r344 170 170 gui::DataPlot1D *mxPlot, *myPlot, *mzPlot; 171 171 172 //for calibration: 172 173 core::Time startCalcOffset; 173 174 bool calibrationDone; 174 core::Vector3Df accOffset,gyrOffset ;175 core::Vector3Df accOffset,gyrOffset,accMax,accMin; 175 176 uint16_t cptOffset; 176 177
Note:
See TracChangeset
for help on using the changeset viewer.