Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

Sensors/Imu/Imu.cpp@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
3:4823d6750629 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "Imu.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 Imu::Imu(ConfigFileWrapper& configFileWrapper) :_configFileWrapper(configFileWrapper)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 _rate = Rate();
joe4465 2:969dfa4f2436 6 _angle = Angle();
joe4465 3:4823d6750629 7 _velocity = Velocity();
joe4465 2:969dfa4f2436 8
joe4465 2:969dfa4f2436 9 _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
joe4465 2:969dfa4f2436 10 _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
joe4465 2:969dfa4f2436 11
joe4465 2:969dfa4f2436 12 Thread::wait(500);
joe4465 2:969dfa4f2436 13 _freeImu.init(true);
joe4465 2:969dfa4f2436 14 Thread::wait(500);
joe4465 2:969dfa4f2436 15
joe4465 2:969dfa4f2436 16 _barometerZeroFilter = new filter(100);
joe4465 3:4823d6750629 17 _barometerFilter = new filter(50);
joe4465 2:969dfa4f2436 18 _barometerZero = 0;
joe4465 2:969dfa4f2436 19 zeroBarometer();
joe4465 2:969dfa4f2436 20
joe4465 3:4823d6750629 21
joe4465 3:4823d6750629 22 _kalmanXVelFilter = new Kalman(0.1, 32, 1, 0);
joe4465 3:4823d6750629 23 _kalmanYVelFilter = new Kalman(0.1, 32, 1, 0);
joe4465 3:4823d6750629 24 _kalmanZVelFilter = new Kalman(0.1, 32, 1, 0);
joe4465 2:969dfa4f2436 25
joe4465 2:969dfa4f2436 26 DEBUG("IMU initialised\r\n");
joe4465 2:969dfa4f2436 27 }
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 Imu::~Imu(){}
joe4465 2:969dfa4f2436 30
joe4465 2:969dfa4f2436 31 Imu::Rate Imu::getRate()
joe4465 2:969dfa4f2436 32 {
joe4465 2:969dfa4f2436 33 float rate[3];
joe4465 2:969dfa4f2436 34 _freeImu.getRate(rate);
joe4465 2:969dfa4f2436 35
joe4465 2:969dfa4f2436 36 float yaw = rate[2];
joe4465 2:969dfa4f2436 37 float pitch = rate[0];
joe4465 2:969dfa4f2436 38 float roll = rate[1];
joe4465 2:969dfa4f2436 39 rate[0] = yaw;
joe4465 2:969dfa4f2436 40 rate[1] = pitch;
joe4465 2:969dfa4f2436 41 rate[2] = roll;
joe4465 2:969dfa4f2436 42
joe4465 2:969dfa4f2436 43 _rate.yaw = rate[0];
joe4465 2:969dfa4f2436 44 _rate.pitch = rate[1];
joe4465 2:969dfa4f2436 45 _rate.roll = rate[2];
joe4465 2:969dfa4f2436 46
joe4465 2:969dfa4f2436 47 return _rate;
joe4465 2:969dfa4f2436 48 }
joe4465 2:969dfa4f2436 49
joe4465 2:969dfa4f2436 50 Imu::Angle Imu::getAngle(bool bias)
joe4465 2:969dfa4f2436 51 {
joe4465 3:4823d6750629 52 //Get raw accel data
joe4465 2:969dfa4f2436 53 float values[9];
joe4465 2:969dfa4f2436 54 _freeImu.getValues(values);
joe4465 2:969dfa4f2436 55
joe4465 3:4823d6750629 56 //Update kalman filter with raw accel data
joe4465 3:4823d6750629 57 _kalmanXVelFilter->update(1, values[0]);
joe4465 3:4823d6750629 58 _kalmanYVelFilter->update(1, values[1]);
joe4465 3:4823d6750629 59 _kalmanZVelFilter->update(1, values[2]);
joe4465 2:969dfa4f2436 60
joe4465 2:969dfa4f2436 61 //Get angle
joe4465 2:969dfa4f2436 62 float angle[3];
joe4465 2:969dfa4f2436 63 _freeImu.getYawPitchRoll(angle);
joe4465 2:969dfa4f2436 64
joe4465 3:4823d6750629 65 //Swap orientation
joe4465 2:969dfa4f2436 66 float yaw = -angle[0];
joe4465 2:969dfa4f2436 67 float pitch = angle[2];
joe4465 2:969dfa4f2436 68 float roll = -angle[1];
joe4465 2:969dfa4f2436 69 angle[0] = yaw;
joe4465 2:969dfa4f2436 70 angle[1] = pitch;
joe4465 2:969dfa4f2436 71 angle[2] = roll;
joe4465 2:969dfa4f2436 72
joe4465 2:969dfa4f2436 73 if(bias == true)
joe4465 2:969dfa4f2436 74 {
joe4465 2:969dfa4f2436 75 _angle.yaw = angle[0];
joe4465 2:969dfa4f2436 76 _angle.pitch = angle[1];//; - _accelZeroPitch;
joe4465 2:969dfa4f2436 77 _angle.roll = angle[2];//; - _accelZeroRoll;
joe4465 2:969dfa4f2436 78 }
joe4465 2:969dfa4f2436 79 else
joe4465 2:969dfa4f2436 80 {
joe4465 2:969dfa4f2436 81 _angle.yaw = angle[0];
joe4465 2:969dfa4f2436 82 _angle.pitch = angle[1];
joe4465 2:969dfa4f2436 83 _angle.roll = angle[2];
joe4465 2:969dfa4f2436 84 }
joe4465 2:969dfa4f2436 85
joe4465 2:969dfa4f2436 86 return _angle;
joe4465 2:969dfa4f2436 87 }
joe4465 2:969dfa4f2436 88
joe4465 2:969dfa4f2436 89 double Imu::getAltitude()
joe4465 2:969dfa4f2436 90 {
joe4465 2:969dfa4f2436 91 float altitude = _barometerFilter->process(_freeImu.getBaroAlt());
joe4465 2:969dfa4f2436 92 float normalAltitude = (altitude - _barometerZero);
joe4465 2:969dfa4f2436 93 return (normalAltitude * 100);
joe4465 2:969dfa4f2436 94 }
joe4465 2:969dfa4f2436 95
joe4465 4:9ffbf9101992 96 Imu::Acceleration Imu::getAcceleration()
joe4465 4:9ffbf9101992 97 {
joe4465 4:9ffbf9101992 98 //Get raw accel data
joe4465 4:9ffbf9101992 99 float values[9];
joe4465 4:9ffbf9101992 100 _freeImu.getValues(values);
joe4465 4:9ffbf9101992 101
joe4465 4:9ffbf9101992 102 Acceleration acceleration;
joe4465 4:9ffbf9101992 103 acceleration.x = values[0];
joe4465 4:9ffbf9101992 104 acceleration.y = values[1];
joe4465 4:9ffbf9101992 105 acceleration.z = values[2];
joe4465 4:9ffbf9101992 106
joe4465 4:9ffbf9101992 107 return acceleration;
joe4465 4:9ffbf9101992 108 }
joe4465 4:9ffbf9101992 109
joe4465 3:4823d6750629 110 Imu::Velocity Imu::getVelocity(float time)
joe4465 2:969dfa4f2436 111 {
joe4465 3:4823d6750629 112 //Get quaternion
joe4465 2:969dfa4f2436 113 float q[4];
joe4465 2:969dfa4f2436 114 _freeImu.getQ(q);
joe4465 2:969dfa4f2436 115
joe4465 2:969dfa4f2436 116 //Extract accelerometer data
joe4465 2:969dfa4f2436 117 float acc[3];
joe4465 3:4823d6750629 118 acc[0]= _kalmanXVelFilter->getEstimated();
joe4465 3:4823d6750629 119 acc[1]= _kalmanYVelFilter->getEstimated();
joe4465 3:4823d6750629 120 acc[2]= _kalmanZVelFilter->getEstimated();
joe4465 2:969dfa4f2436 121
joe4465 2:969dfa4f2436 122 //Gravity compensate
joe4465 2:969dfa4f2436 123 _freeImu.gravityCompensateAcc(acc, q);
joe4465 2:969dfa4f2436 124
joe4465 3:4823d6750629 125 //Convert acceleration to velocity
joe4465 3:4823d6750629 126 float xAcceleration = 0;
joe4465 3:4823d6750629 127 if(acc[0] < -0.01 || acc[0] > 0.01) xAcceleration = acc[0] * 9.8 * 100;
joe4465 3:4823d6750629 128 _velocity.x += xAcceleration * time;
joe4465 3:4823d6750629 129
joe4465 3:4823d6750629 130 float yAcceleration = 0;
joe4465 3:4823d6750629 131 if(acc[1] < -0.01 || acc[1] > 0.01) yAcceleration = acc[1] * 9.8 * 100;
joe4465 3:4823d6750629 132 _velocity.y += yAcceleration * time;
joe4465 3:4823d6750629 133
joe4465 2:969dfa4f2436 134 float zAcceleration = 0;
joe4465 2:969dfa4f2436 135 if(acc[2] < -0.01 || acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
joe4465 3:4823d6750629 136 _velocity.z += zAcceleration * time;
joe4465 2:969dfa4f2436 137
joe4465 2:969dfa4f2436 138 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 139 }
joe4465 2:969dfa4f2436 140
joe4465 3:4823d6750629 141 Imu::Velocity Imu::getVelocity()
joe4465 2:969dfa4f2436 142 {
joe4465 2:969dfa4f2436 143 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 144 }
joe4465 2:969dfa4f2436 145
joe4465 2:969dfa4f2436 146 void Imu::zeroGyro()
joe4465 2:969dfa4f2436 147 {
joe4465 2:969dfa4f2436 148 _freeImu.zeroGyro();
joe4465 2:969dfa4f2436 149 }
joe4465 2:969dfa4f2436 150
joe4465 3:4823d6750629 151 void Imu::setCurrentVelocity(Velocity velocity)
joe4465 2:969dfa4f2436 152 {
joe4465 2:969dfa4f2436 153 _velocity = velocity;
joe4465 2:969dfa4f2436 154 }
joe4465 2:969dfa4f2436 155
joe4465 2:969dfa4f2436 156 void Imu::zeroBarometer()
joe4465 2:969dfa4f2436 157 {
joe4465 3:4823d6750629 158 //DEBUG("About to start Barometer zero\r\n");
joe4465 3:4823d6750629 159 //Thread::wait(5000);
joe4465 3:4823d6750629 160
joe4465 3:4823d6750629 161 for(int i = 0; i < 1000; i++)
joe4465 2:969dfa4f2436 162 {
joe4465 2:969dfa4f2436 163 _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
joe4465 2:969dfa4f2436 164 }
joe4465 2:969dfa4f2436 165 DEBUG("Barometer zero %f\r\n", _barometerZero);
joe4465 2:969dfa4f2436 166 }
joe4465 2:969dfa4f2436 167
joe4465 2:969dfa4f2436 168 void Imu::enable(bool enable)
joe4465 2:969dfa4f2436 169 {
joe4465 2:969dfa4f2436 170 _freeImu.sample(enable);
joe4465 2:969dfa4f2436 171 }
joe4465 2:969dfa4f2436 172
joe4465 2:969dfa4f2436 173 void Imu::zeroAccel()
joe4465 2:969dfa4f2436 174 {
joe4465 2:969dfa4f2436 175 Imu::Angle angle = getAngle(false);
joe4465 2:969dfa4f2436 176 _accelZeroPitch = angle.pitch;
joe4465 2:969dfa4f2436 177 _accelZeroRoll = angle.roll;
joe4465 2:969dfa4f2436 178 //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
joe4465 2:969dfa4f2436 179 //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
joe4465 2:969dfa4f2436 180 //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll);
joe4465 2:969dfa4f2436 181 //_configFileWrapper.saveSettings();
joe4465 2:969dfa4f2436 182 }