Diff: Sensors/Imu/Imu.cpp

Revision:

2:969dfa4f2436

Parent:

0:c6a85bb2a827

Child:

3:4823d6750629

--- a/Sensors/Imu/Imu.cpp	Wed Mar 04 18:53:43 2015 +0000
+++ b/Sensors/Imu/Imu.cpp	Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,152 @@
+#include "Imu.h"
+
+Imu::Imu(ConfigFileWrapper& configFileWrapper) :_configFileWrapper(configFileWrapper)
+{
+    _rate = Rate();
+    _angle = Angle();
+    
+    _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
+    _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
+    
+    Thread::wait(500);
+    _freeImu.init(true);
+    Thread::wait(500);
+    
+    _barometerZeroFilter = new filter(100);
+    _barometerFilter = new filter(5);
+    
+    _barometerZero = 0;
+    zeroBarometer();
+    
+    _velocity = 0;
+    _kalmanFilter = new Kalman(0.1, 32, 1, 0);
+    
+    DEBUG("IMU initialised\r\n");
+}
+
+Imu::~Imu(){}
+
+Imu::Rate Imu::getRate()
+{
+    float rate[3];
+    _freeImu.getRate(rate);
+
+    float yaw = rate[2];
+    float pitch = rate[0];
+    float roll = rate[1];
+    rate[0] = yaw;
+    rate[1] = pitch;
+    rate[2] = roll;
+    
+    _rate.yaw = rate[0];
+    _rate.pitch = rate[1];
+    _rate.roll = rate[2];
+    
+    return _rate;
+}
+
+Imu::Angle Imu::getAngle(bool bias)
+{
+    //Filter Z 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]);
+    
+    //Get angle
+    float angle[3];
+    _freeImu.getYawPitchRoll(angle);
+    
+    float yaw = -angle[0];
+    float pitch = angle[2];
+    float roll = -angle[1];
+    angle[0] = yaw;
+    angle[1] = pitch;
+    angle[2] = roll;
+    
+    if(bias == true)
+    {
+        _angle.yaw = angle[0];
+        _angle.pitch = angle[1];//; - _accelZeroPitch;
+        _angle.roll = angle[2];//; - _accelZeroRoll;
+    }
+    else
+    {
+        _angle.yaw = angle[0];
+        _angle.pitch = angle[1];
+        _angle.roll = angle[2];
+    }
+    
+    return _angle;
+}
+
+double Imu::getAltitude()
+{
+    float altitude = _barometerFilter->process(_freeImu.getBaroAlt());
+    float normalAltitude = (altitude - _barometerZero);
+    return (normalAltitude * 100);
+}
+
+float Imu::getVelocity(float time)
+{
+    //Get quat
+    float q[4];
+    _freeImu.getQ(q);
+
+    //Extract accelerometer data
+    float acc[3];
+    acc[0]= 0; //x
+    acc[1]= 0; //y
+    acc[2]= _kalmanFilter->getEstimated();
+    
+    //Gravity compensate
+    _freeImu.gravityCompensateAcc(acc, q);
+    
+    float zAcceleration = 0;
+    if(acc[2] < -0.01 ||  acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
+    
+    _velocity += zAcceleration * time;
+    
+    return _velocity; //cm/s/s
+}
+
+float Imu::getVelocity()
+{
+    return _velocity; //cm/s/s
+}
+
+void Imu::zeroGyro()
+{
+    _freeImu.zeroGyro();
+}
+
+void Imu::setCurrentVelocity(float velocity)
+{
+    _velocity = velocity;
+}
+
+void Imu::zeroBarometer()
+{
+    for(int i = 0; i < 100; i++)
+    {
+        _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
+    }
+    DEBUG("Barometer zero %f\r\n", _barometerZero);
+}
+
+void Imu::enable(bool enable)
+{
+    _freeImu.sample(enable);
+}
+
+void Imu::zeroAccel()
+{
+     Imu::Angle angle = getAngle(false);
+     _accelZeroPitch = angle.pitch;
+     _accelZeroRoll = angle.roll;
+     //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
+     //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
+     //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll); 
+     //_configFileWrapper.saveSettings();
+}
\ No newline at end of file