- Timestamp:
- Dec 12, 2019, 6:11:36 PM (5 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/Imu.cpp
r340 r343 35 35 Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) { 36 36 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; 37 46 38 47 // station sol … … 82 91 } 83 92 93 void 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 84 125 GroupBox *Imu::GetGroupBox(void) const { return setupGroupbox; } 85 126 -
trunk/lib/FlairSensorActuator/src/Imu.h
r340 r343 147 147 */ 148 148 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); 149 160 150 161 private: … … 158 169 gui::DataPlot1D *gxPlot, *gyPlot, *gzPlot; 159 170 gui::DataPlot1D *mxPlot, *myPlot, *mzPlot; 171 172 core::Time startCalcOffset; 173 bool calibrationDone; 174 core::Vector3Df accOffset,gyrOffset; 175 uint16_t cptOffset; 176 160 177 }; 161 178 } // end namespace sensor
Note:
See TracChangeset
for help on using the changeset viewer.