Diff: Sensors/Imu/Imu.cpp

Revision:

3:4823d6750629

Parent:

2:969dfa4f2436

Child:

4:9ffbf9101992

--- a/Sensors/Imu/Imu.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Imu/Imu.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -4,6 +4,7 @@
 {
     _rate = Rate();
     _angle = Angle();
+    _velocity = Velocity();
     
     _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
     _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
@@ -13,13 +14,14 @@
     Thread::wait(500);
     
     _barometerZeroFilter = new filter(100);
-    _barometerFilter = new filter(5);
-    
+    _barometerFilter = new filter(50);
     _barometerZero = 0;
     zeroBarometer();
     
-    _velocity = 0;
-    _kalmanFilter = new Kalman(0.1, 32, 1, 0);
+    
+    _kalmanXVelFilter = new Kalman(0.1, 32, 1, 0);
+    _kalmanYVelFilter = new Kalman(0.1, 32, 1, 0);
+    _kalmanZVelFilter = new Kalman(0.1, 32, 1, 0);
     
     DEBUG("IMU initialised\r\n");
 }
@@ -47,17 +49,20 @@
 
 Imu::Angle Imu::getAngle(bool bias)
 {
-    //Filter Z accel data
+    //Get raw accel data
     float values[9];
     _freeImu.getValues(values);
-    _kalmanFilter->update(1, values[2]);
     
-    //printf("%1.6f, %1.6f, %1.6f\n", values[0], values[1], values[2]);
+    //Update kalman filter with raw accel data
+    _kalmanXVelFilter->update(1, values[0]);
+    _kalmanYVelFilter->update(1, values[1]);
+    _kalmanZVelFilter->update(1, values[2]);
     
     //Get angle
     float angle[3];
     _freeImu.getYawPitchRoll(angle);
     
+    //Swap orientation
     float yaw = -angle[0];
     float pitch = angle[2];
     float roll = -angle[1];
@@ -88,30 +93,38 @@
     return (normalAltitude * 100);
 }
 
-float Imu::getVelocity(float time)
+Imu::Velocity Imu::getVelocity(float time)
 {
-    //Get quat
+    //Get quaternion
     float q[4];
     _freeImu.getQ(q);
 
     //Extract accelerometer data
     float acc[3];
-    acc[0]= 0; //x
-    acc[1]= 0; //y
-    acc[2]= _kalmanFilter->getEstimated();
+    acc[0]= _kalmanXVelFilter->getEstimated();
+    acc[1]= _kalmanYVelFilter->getEstimated();
+    acc[2]= _kalmanZVelFilter->getEstimated();
     
     //Gravity compensate
     _freeImu.gravityCompensateAcc(acc, q);
     
+    //Convert acceleration to velocity
+    float xAcceleration = 0;
+    if(acc[0] < -0.01 ||  acc[0] > 0.01) xAcceleration = acc[0] * 9.8 * 100;
+    _velocity.x += xAcceleration * time;
+    
+    float yAcceleration = 0;
+    if(acc[1] < -0.01 ||  acc[1] > 0.01) yAcceleration = acc[1] * 9.8 * 100;
+    _velocity.y += yAcceleration * time;
+    
     float zAcceleration = 0;
     if(acc[2] < -0.01 ||  acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
-    
-    _velocity += zAcceleration * time;
+    _velocity.z += zAcceleration * time;
     
     return _velocity; //cm/s/s
 }
 
-float Imu::getVelocity()
+Imu::Velocity Imu::getVelocity()
 {
     return _velocity; //cm/s/s
 }
@@ -121,14 +134,17 @@
     _freeImu.zeroGyro();
 }
 
-void Imu::setCurrentVelocity(float velocity)
+void Imu::setCurrentVelocity(Velocity velocity)
 {
     _velocity = velocity;
 }
 
 void Imu::zeroBarometer()
 {
-    for(int i = 0; i < 100; i++)
+    //DEBUG("About to start Barometer zero\r\n");
+    //Thread::wait(5000);
+    
+    for(int i = 0; i < 1000; i++)
     {
         _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
     }