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@2:969dfa4f2436, 2015-04-01 (annotated)

Committer:
joe4465
Date:
Wed Apr 01 11:19:21 2015 +0000
Revision:
2:969dfa4f2436
Parent:
0:c6a85bb2a827
Child:
3:4823d6750629
Altitude hold with 2 PID's working. Exported to offline compiler

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 2:969dfa4f2436 7
joe4465 2:969dfa4f2436 8 _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
joe4465 2:969dfa4f2436 9 _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
joe4465 2:969dfa4f2436 10
joe4465 2:969dfa4f2436 11 Thread::wait(500);
joe4465 2:969dfa4f2436 12 _freeImu.init(true);
joe4465 2:969dfa4f2436 13 Thread::wait(500);
joe4465 2:969dfa4f2436 14
joe4465 2:969dfa4f2436 15 _barometerZeroFilter = new filter(100);
joe4465 2:969dfa4f2436 16 _barometerFilter = new filter(5);
joe4465 2:969dfa4f2436 17
joe4465 2:969dfa4f2436 18 _barometerZero = 0;
joe4465 2:969dfa4f2436 19 zeroBarometer();
joe4465 2:969dfa4f2436 20
joe4465 2:969dfa4f2436 21 _velocity = 0;
joe4465 2:969dfa4f2436 22 _kalmanFilter = new Kalman(0.1, 32, 1, 0);
joe4465 2:969dfa4f2436 23
joe4465 2:969dfa4f2436 24 DEBUG("IMU initialised\r\n");
joe4465 2:969dfa4f2436 25 }
joe4465 2:969dfa4f2436 26
joe4465 2:969dfa4f2436 27 Imu::~Imu(){}
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 Imu::Rate Imu::getRate()
joe4465 2:969dfa4f2436 30 {
joe4465 2:969dfa4f2436 31 float rate[3];
joe4465 2:969dfa4f2436 32 _freeImu.getRate(rate);
joe4465 2:969dfa4f2436 33
joe4465 2:969dfa4f2436 34 float yaw = rate[2];
joe4465 2:969dfa4f2436 35 float pitch = rate[0];
joe4465 2:969dfa4f2436 36 float roll = rate[1];
joe4465 2:969dfa4f2436 37 rate[0] = yaw;
joe4465 2:969dfa4f2436 38 rate[1] = pitch;
joe4465 2:969dfa4f2436 39 rate[2] = roll;
joe4465 2:969dfa4f2436 40
joe4465 2:969dfa4f2436 41 _rate.yaw = rate[0];
joe4465 2:969dfa4f2436 42 _rate.pitch = rate[1];
joe4465 2:969dfa4f2436 43 _rate.roll = rate[2];
joe4465 2:969dfa4f2436 44
joe4465 2:969dfa4f2436 45 return _rate;
joe4465 2:969dfa4f2436 46 }
joe4465 2:969dfa4f2436 47
joe4465 2:969dfa4f2436 48 Imu::Angle Imu::getAngle(bool bias)
joe4465 2:969dfa4f2436 49 {
joe4465 2:969dfa4f2436 50 //Filter Z accel data
joe4465 2:969dfa4f2436 51 float values[9];
joe4465 2:969dfa4f2436 52 _freeImu.getValues(values);
joe4465 2:969dfa4f2436 53 _kalmanFilter->update(1, values[2]);
joe4465 2:969dfa4f2436 54
joe4465 2:969dfa4f2436 55 //printf("%1.6f, %1.6f, %1.6f\n", values[0], values[1], values[2]);
joe4465 2:969dfa4f2436 56
joe4465 2:969dfa4f2436 57 //Get angle
joe4465 2:969dfa4f2436 58 float angle[3];
joe4465 2:969dfa4f2436 59 _freeImu.getYawPitchRoll(angle);
joe4465 2:969dfa4f2436 60
joe4465 2:969dfa4f2436 61 float yaw = -angle[0];
joe4465 2:969dfa4f2436 62 float pitch = angle[2];
joe4465 2:969dfa4f2436 63 float roll = -angle[1];
joe4465 2:969dfa4f2436 64 angle[0] = yaw;
joe4465 2:969dfa4f2436 65 angle[1] = pitch;
joe4465 2:969dfa4f2436 66 angle[2] = roll;
joe4465 2:969dfa4f2436 67
joe4465 2:969dfa4f2436 68 if(bias == true)
joe4465 2:969dfa4f2436 69 {
joe4465 2:969dfa4f2436 70 _angle.yaw = angle[0];
joe4465 2:969dfa4f2436 71 _angle.pitch = angle[1];//; - _accelZeroPitch;
joe4465 2:969dfa4f2436 72 _angle.roll = angle[2];//; - _accelZeroRoll;
joe4465 2:969dfa4f2436 73 }
joe4465 2:969dfa4f2436 74 else
joe4465 2:969dfa4f2436 75 {
joe4465 2:969dfa4f2436 76 _angle.yaw = angle[0];
joe4465 2:969dfa4f2436 77 _angle.pitch = angle[1];
joe4465 2:969dfa4f2436 78 _angle.roll = angle[2];
joe4465 2:969dfa4f2436 79 }
joe4465 2:969dfa4f2436 80
joe4465 2:969dfa4f2436 81 return _angle;
joe4465 2:969dfa4f2436 82 }
joe4465 2:969dfa4f2436 83
joe4465 2:969dfa4f2436 84 double Imu::getAltitude()
joe4465 2:969dfa4f2436 85 {
joe4465 2:969dfa4f2436 86 float altitude = _barometerFilter->process(_freeImu.getBaroAlt());
joe4465 2:969dfa4f2436 87 float normalAltitude = (altitude - _barometerZero);
joe4465 2:969dfa4f2436 88 return (normalAltitude * 100);
joe4465 2:969dfa4f2436 89 }
joe4465 2:969dfa4f2436 90
joe4465 2:969dfa4f2436 91 float Imu::getVelocity(float time)
joe4465 2:969dfa4f2436 92 {
joe4465 2:969dfa4f2436 93 //Get quat
joe4465 2:969dfa4f2436 94 float q[4];
joe4465 2:969dfa4f2436 95 _freeImu.getQ(q);
joe4465 2:969dfa4f2436 96
joe4465 2:969dfa4f2436 97 //Extract accelerometer data
joe4465 2:969dfa4f2436 98 float acc[3];
joe4465 2:969dfa4f2436 99 acc[0]= 0; //x
joe4465 2:969dfa4f2436 100 acc[1]= 0; //y
joe4465 2:969dfa4f2436 101 acc[2]= _kalmanFilter->getEstimated();
joe4465 2:969dfa4f2436 102
joe4465 2:969dfa4f2436 103 //Gravity compensate
joe4465 2:969dfa4f2436 104 _freeImu.gravityCompensateAcc(acc, q);
joe4465 2:969dfa4f2436 105
joe4465 2:969dfa4f2436 106 float zAcceleration = 0;
joe4465 2:969dfa4f2436 107 if(acc[2] < -0.01 || acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
joe4465 2:969dfa4f2436 108
joe4465 2:969dfa4f2436 109 _velocity += zAcceleration * time;
joe4465 2:969dfa4f2436 110
joe4465 2:969dfa4f2436 111 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 112 }
joe4465 2:969dfa4f2436 113
joe4465 2:969dfa4f2436 114 float Imu::getVelocity()
joe4465 2:969dfa4f2436 115 {
joe4465 2:969dfa4f2436 116 return _velocity; //cm/s/s
joe4465 2:969dfa4f2436 117 }
joe4465 2:969dfa4f2436 118
joe4465 2:969dfa4f2436 119 void Imu::zeroGyro()
joe4465 2:969dfa4f2436 120 {
joe4465 2:969dfa4f2436 121 _freeImu.zeroGyro();
joe4465 2:969dfa4f2436 122 }
joe4465 2:969dfa4f2436 123
joe4465 2:969dfa4f2436 124 void Imu::setCurrentVelocity(float velocity)
joe4465 2:969dfa4f2436 125 {
joe4465 2:969dfa4f2436 126 _velocity = velocity;
joe4465 2:969dfa4f2436 127 }
joe4465 2:969dfa4f2436 128
joe4465 2:969dfa4f2436 129 void Imu::zeroBarometer()
joe4465 2:969dfa4f2436 130 {
joe4465 2:969dfa4f2436 131 for(int i = 0; i < 100; i++)
joe4465 2:969dfa4f2436 132 {
joe4465 2:969dfa4f2436 133 _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
joe4465 2:969dfa4f2436 134 }
joe4465 2:969dfa4f2436 135 DEBUG("Barometer zero %f\r\n", _barometerZero);
joe4465 2:969dfa4f2436 136 }
joe4465 2:969dfa4f2436 137
joe4465 2:969dfa4f2436 138 void Imu::enable(bool enable)
joe4465 2:969dfa4f2436 139 {
joe4465 2:969dfa4f2436 140 _freeImu.sample(enable);
joe4465 2:969dfa4f2436 141 }
joe4465 2:969dfa4f2436 142
joe4465 2:969dfa4f2436 143 void Imu::zeroAccel()
joe4465 2:969dfa4f2436 144 {
joe4465 2:969dfa4f2436 145 Imu::Angle angle = getAngle(false);
joe4465 2:969dfa4f2436 146 _accelZeroPitch = angle.pitch;
joe4465 2:969dfa4f2436 147 _accelZeroRoll = angle.roll;
joe4465 2:969dfa4f2436 148 //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
joe4465 2:969dfa4f2436 149 //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
joe4465 2:969dfa4f2436 150 //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll);
joe4465 2:969dfa4f2436 151 //_configFileWrapper.saveSettings();
joe4465 2:969dfa4f2436 152 }