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?
User | Revision | Line number | New 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 | } |