Imu.cpp

00001 #include "Imu.h"
00002 
00003 Imu::Imu(ConfigFileWrapper& configFileWrapper) :_configFileWrapper(configFileWrapper)
00004 {
00005     _rate = Rate();
00006     _angle = Angle();
00007     _velocity = Velocity();
00008     
00009     _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
00010     _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
00011     
00012     Thread::wait(500);
00013     _freeImu.init(true);
00014     Thread::wait(500);
00015     
00016     _barometerZeroFilter = new filter(100);
00017     _barometerFilter = new filter(50);
00018     _barometerZero = 0;
00019     zeroBarometer();
00020     
00021     
00022     _kalmanXVelFilter = new Kalman(0.1, 32, 1, 0);
00023     _kalmanYVelFilter = new Kalman(0.1, 32, 1, 0);
00024     _kalmanZVelFilter = new Kalman(0.1, 32, 1, 0);
00025     
00026     DEBUG("IMU initialised\r\n");
00027 }
00028 
00029 Imu::~Imu(){}
00030 
00031 Imu::Rate Imu::getRate()
00032 {
00033     float rate[3];
00034     _freeImu.getRate(rate);
00035 
00036     float yaw = rate[2];
00037     float pitch = rate[0];
00038     float roll = rate[1];
00039     rate[0] = yaw;
00040     rate[1] = pitch;
00041     rate[2] = roll;
00042     
00043     _rate.yaw = rate[0];
00044     _rate.pitch = rate[1];
00045     _rate.roll = rate[2];
00046     
00047     return _rate;
00048 }
00049 
00050 Imu::Angle Imu::getAngle(bool bias)
00051 {
00052     //Get raw accel data
00053     float values[9];
00054     _freeImu.getValues(values);
00055     
00056     //Update kalman filter with raw accel data
00057     _kalmanXVelFilter->update(1, values[0]);
00058     _kalmanYVelFilter->update(1, values[1]);
00059     _kalmanZVelFilter->update(1, values[2]);
00060     
00061     //Get angle
00062     float angle[3];
00063     _freeImu.getYawPitchRoll(angle);
00064     
00065     //Swap orientation
00066     float yaw = -angle[0];
00067     float pitch = angle[2];
00068     float roll = -angle[1];
00069     angle[0] = yaw;
00070     angle[1] = pitch;
00071     angle[2] = roll;
00072     
00073     if(bias == true)
00074     {
00075         _angle.yaw = angle[0];
00076         _angle.pitch = angle[1];//; - _accelZeroPitch;
00077         _angle.roll = angle[2];//; - _accelZeroRoll;
00078     }
00079     else
00080     {
00081         _angle.yaw = angle[0];
00082         _angle.pitch = angle[1];
00083         _angle.roll = angle[2];
00084     }
00085     
00086     return _angle;
00087 }
00088 
00089 double Imu::getAltitude()
00090 {
00091     float altitude = _barometerFilter->process(_freeImu.getBaroAlt());
00092     float normalAltitude = (altitude - _barometerZero);
00093     return (normalAltitude * 100);
00094 }
00095 
00096 Imu::Acceleration Imu::getAcceleration()
00097 {
00098     //Get raw accel data
00099     float values[9];
00100     _freeImu.getValues(values);
00101     
00102     Acceleration acceleration;
00103     acceleration.x = values[0];
00104     acceleration.y = values[1];
00105     acceleration.z = values[2];
00106     
00107     return acceleration;
00108 }
00109 
00110 Imu::Velocity Imu::getVelocity(float time)
00111 {
00112     //Get quaternion
00113     float q[4];
00114     _freeImu.getQ(q);
00115 
00116     //Extract accelerometer data
00117     float acc[3];
00118     acc[0]= _kalmanXVelFilter->getEstimated();
00119     acc[1]= _kalmanYVelFilter->getEstimated();
00120     acc[2]= _kalmanZVelFilter->getEstimated();
00121     
00122     //Gravity compensate
00123     _freeImu.gravityCompensateAcc(acc, q);
00124     
00125     //Convert acceleration to velocity
00126     float xAcceleration = 0;
00127     if(acc[0] < -0.01 ||  acc[0] > 0.01) xAcceleration = acc[0] * 9.8 * 100;
00128     _velocity.x += xAcceleration * time;
00129     
00130     float yAcceleration = 0;
00131     if(acc[1] < -0.01 ||  acc[1] > 0.01) yAcceleration = acc[1] * 9.8 * 100;
00132     _velocity.y += yAcceleration * time;
00133     
00134     float zAcceleration = 0;
00135     if(acc[2] < -0.01 ||  acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
00136     _velocity.z += zAcceleration * time;
00137     
00138     return _velocity; //cm/s/s
00139 }
00140 
00141 Imu::Velocity Imu::getVelocity()
00142 {
00143     return _velocity; //cm/s/s
00144 }
00145 
00146 void Imu::zeroGyro()
00147 {
00148     _freeImu.zeroGyro();
00149 }
00150 
00151 void Imu::setCurrentVelocity(Velocity velocity)
00152 {
00153     _velocity = velocity;
00154 }
00155 
00156 void Imu::zeroBarometer()
00157 {
00158     //DEBUG("About to start Barometer zero\r\n");
00159     //Thread::wait(5000);
00160     
00161     for(int i = 0; i < 1000; i++)
00162     {
00163         _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
00164     }
00165     DEBUG("Barometer zero %f\r\n", _barometerZero);
00166 }
00167 
00168 void Imu::enable(bool enable)
00169 {
00170     _freeImu.sample(enable);
00171 }
00172 
00173 void Imu::zeroAccel()
00174 {
00175      Imu::Angle angle = getAngle(false);
00176      _accelZeroPitch = angle.pitch;
00177      _accelZeroRoll = angle.roll;
00178      //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
00179      //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
00180      //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll); 
00181      //_configFileWrapper.saveSettings();
00182 }