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@3:4823d6750629, 2015-04-24 (annotated)

Committer:
joe4465
Date:
Fri Apr 24 16:50:20 2015 +0000
Revision:
3:4823d6750629
Parent:
2:969dfa4f2436
Child:
4:9ffbf9101992
Altitude mode working.

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 3:4823d6750629 96 Imu::Velocity Imu::getVelocity(float time)
joe4465 2:969dfa4f2436 97 {
joe4465 3:4823d6750629 98 //Get quaternion
joe4465 2:969dfa4f2436 99 float q[4];
joe4465 2:969dfa4f2436 100 _freeImu.getQ(q);
joe4465 2:969dfa4f2436 101
joe4465 2:969dfa4f2436 102 //Extract accelerometer data
joe4465 2:969dfa4f2436 103 float acc[3];
joe4465 3:4823d6750629 104 acc[0]= _kalmanXVelFilter->getEstimated();
joe4465 3:4823d6750629 105 acc[1]= _kalmanYVelFilter->getEstimated();
joe4465 3:4823d6750629 106 acc[2]= _kalmanZVelFilter->getEstimated();
joe4465 2:969dfa4f2436 107
joe4465 2:969dfa4f2436 108 //Gravity compensate
joe4465 2:969dfa4f2436 109 _freeImu.gravityCompensateAcc(acc, q);
joe4465 2:969dfa4f2436 110
joe4465 3:4823d6750629 111 //Convert acceleration to velocity
joe4465 3:4823d6750629 112 float xAcceleration = 0;
joe4465 3:4823d6750629 113 if(acc[0] < -0.01 || acc[0] > 0.01) xAcceleration = acc[0] * 9.8 * 100;
joe4465 3:4823d6750629 114 _velocity.x += xAcceleration * time;
joe4465 3:4823d6750629 115
joe4465 3:4823d6750629 116 float yAcceleration = 0;
joe4465 3:4823d6750629 117 if(acc[1] < -0.01 || acc[1] > 0.01) yAcceleration = acc[1] * 9.8 * 100;
joe4465 3:4823d6750629 118 _velocity.y += yAcceleration * time;
joe4465 3:4823d6750629 119
joe4465 2:969dfa4f2436 120 float zAcceleration = 0;
joe4465 2:969dfa4f2436 121 if(acc[2] < -0.01 || acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
joe4465 3:4823d6750629 122 _velocity.z += zAcceleration * time;
joe4465 2:969dfa4f2436 123
joe4465 2:969dfa4f2436 124 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 125 }
joe4465 2:969dfa4f2436 126
joe4465 3:4823d6750629 127 Imu::Velocity Imu::getVelocity()
joe4465 2:969dfa4f2436 128 {
joe4465 2:969dfa4f2436 129 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 130 }
joe4465 2:969dfa4f2436 131
joe4465 2:969dfa4f2436 132 void Imu::zeroGyro()
joe4465 2:969dfa4f2436 133 {
joe4465 2:969dfa4f2436 134 _freeImu.zeroGyro();
joe4465 2:969dfa4f2436 135 }
joe4465 2:969dfa4f2436 136
joe4465 3:4823d6750629 137 void Imu::setCurrentVelocity(Velocity velocity)
joe4465 2:969dfa4f2436 138 {
joe4465 2:969dfa4f2436 139 _velocity = velocity;
joe4465 2:969dfa4f2436 140 }
joe4465 2:969dfa4f2436 141
joe4465 2:969dfa4f2436 142 void Imu::zeroBarometer()
joe4465 2:969dfa4f2436 143 {
joe4465 3:4823d6750629 144 //DEBUG("About to start Barometer zero\r\n");
joe4465 3:4823d6750629 145 //Thread::wait(5000);
joe4465 3:4823d6750629 146
joe4465 3:4823d6750629 147 for(int i = 0; i < 1000; i++)
joe4465 2:969dfa4f2436 148 {
joe4465 2:969dfa4f2436 149 _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
joe4465 2:969dfa4f2436 150 }
joe4465 2:969dfa4f2436 151 DEBUG("Barometer zero %f\r\n", _barometerZero);
joe4465 2:969dfa4f2436 152 }
joe4465 2:969dfa4f2436 153
joe4465 2:969dfa4f2436 154 void Imu::enable(bool enable)
joe4465 2:969dfa4f2436 155 {
joe4465 2:969dfa4f2436 156 _freeImu.sample(enable);
joe4465 2:969dfa4f2436 157 }
joe4465 2:969dfa4f2436 158
joe4465 2:969dfa4f2436 159 void Imu::zeroAccel()
joe4465 2:969dfa4f2436 160 {
joe4465 2:969dfa4f2436 161 Imu::Angle angle = getAngle(false);
joe4465 2:969dfa4f2436 162 _accelZeroPitch = angle.pitch;
joe4465 2:969dfa4f2436 163 _accelZeroRoll = angle.roll;
joe4465 2:969dfa4f2436 164 //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
joe4465 2:969dfa4f2436 165 //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
joe4465 2:969dfa4f2436 166 //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll);
joe4465 2:969dfa4f2436 167 //_configFileWrapper.saveSettings();
joe4465 2:969dfa4f2436 168 }