Joseph Roberts / Mbed 2 deprecated QuadCopter

Quadcopter software. Flying - Contact me for more details or a copy of the PC ground station code

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

flightController.h@5:7b7db24ef6eb, 2014-09-22 (annotated)

Committer:
joe4465
Date:
Mon Sep 22 10:16:31 2014 +0000
Revision:
5:7b7db24ef6eb
Parent:
3:82665e39f1ea
Child:
6:4c207e7b1203
Testing with Euler angles instead of YPR

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:0010a5abcc31 1 #include "mbed.h"
joe4465 0:0010a5abcc31 2 #include "rtos.h"
joe4465 0:0010a5abcc31 3 #include "hardware.h"
joe4465 0:0010a5abcc31 4
joe4465 0:0010a5abcc31 5 //Declarations
joe4465 0:0010a5abcc31 6 void GetAttitude();
joe4465 3:82665e39f1ea 7 void FlightControllerTask(void const *n);
joe4465 3:82665e39f1ea 8 void NotFlying();
joe4465 0:0010a5abcc31 9
joe4465 0:0010a5abcc31 10 //Variables
joe4465 3:82665e39f1ea 11 float _yawTarget = 0;
joe4465 3:82665e39f1ea 12 int _notFlying = 0;
joe4465 3:82665e39f1ea 13 float _altitude = 0;
joe4465 3:82665e39f1ea 14 double _basePower = 0;
joe4465 0:0010a5abcc31 15
joe4465 1:045edcf091f3 16 //Timers
joe4465 3:82665e39f1ea 17 RtosTimer *_flightControllerUpdateTimer;
joe4465 1:045edcf091f3 18
joe4465 3:82665e39f1ea 19 // A thread to control flight
joe4465 0:0010a5abcc31 20 void FlightControllerThread(void const *args)
joe4465 3:82665e39f1ea 21 {
joe4465 1:045edcf091f3 22 //Update Timer
joe4465 3:82665e39f1ea 23 _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
joe4465 3:82665e39f1ea 24 int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
joe4465 3:82665e39f1ea 25 _flightControllerUpdateTimer->start(updateTime);
joe4465 0:0010a5abcc31 26
joe4465 0:0010a5abcc31 27 // Wait here forever
joe4465 0:0010a5abcc31 28 Thread::wait(osWaitForever);
joe4465 0:0010a5abcc31 29 }
joe4465 0:0010a5abcc31 30
joe4465 3:82665e39f1ea 31 void FlightControllerTask(void const *n)
joe4465 0:0010a5abcc31 32 {
joe4465 0:0010a5abcc31 33 //Get IMU data
joe4465 0:0010a5abcc31 34 GetAttitude();
joe4465 0:0010a5abcc31 35
joe4465 0:0010a5abcc31 36 //Rate mode
joe4465 0:0010a5abcc31 37 if(_rate == true && _stab == false)
joe4465 0:0010a5abcc31 38 {
joe4465 0:0010a5abcc31 39 //Update rate PID process value with gyro rate
joe4465 0:0010a5abcc31 40 _yawRatePIDController->setProcessValue(_gyroRate[0]);
joe4465 0:0010a5abcc31 41 _pitchRatePIDController->setProcessValue(_gyroRate[1]);
joe4465 0:0010a5abcc31 42 _rollRatePIDController->setProcessValue(_gyroRate[2]);
joe4465 0:0010a5abcc31 43
joe4465 0:0010a5abcc31 44 //Update rate PID set point with desired rate from RC
joe4465 1:045edcf091f3 45 _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
joe4465 1:045edcf091f3 46 _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 47 _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 48
joe4465 0:0010a5abcc31 49 //Compute rate PID outputs
joe4465 0:0010a5abcc31 50 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 1:045edcf091f3 51 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 52 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 3:82665e39f1ea 53
joe4465 3:82665e39f1ea 54 //Set stability PID outputs to 0
joe4465 3:82665e39f1ea 55 _stabPIDControllerOutputs[0] = 0;
joe4465 3:82665e39f1ea 56 _stabPIDControllerOutputs[1] = 0;
joe4465 3:82665e39f1ea 57 _stabPIDControllerOutputs[2] = 0;
joe4465 0:0010a5abcc31 58 }
joe4465 0:0010a5abcc31 59 //Stability mode
joe4465 3:82665e39f1ea 60 else if(_rate == false && _stab == true)
joe4465 0:0010a5abcc31 61 {
joe4465 0:0010a5abcc31 62 //Update stab PID process value with ypr
joe4465 0:0010a5abcc31 63 _yawStabPIDController->setProcessValue(_ypr[0]);
joe4465 0:0010a5abcc31 64 _pitchStabPIDController->setProcessValue(_ypr[1]);
joe4465 0:0010a5abcc31 65 _rollStabPIDController->setProcessValue(_ypr[2]);
joe4465 0:0010a5abcc31 66
joe4465 0:0010a5abcc31 67 //Update stab PID set point with desired angle from RC
joe4465 0:0010a5abcc31 68 _yawStabPIDController->setSetPoint(_yawTarget);
joe4465 1:045edcf091f3 69 _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 70 _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 71
joe4465 0:0010a5abcc31 72 //Compute stab PID outputs
joe4465 0:0010a5abcc31 73 _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
joe4465 1:045edcf091f3 74 _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
joe4465 0:0010a5abcc31 75 _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
joe4465 0:0010a5abcc31 76
joe4465 1:045edcf091f3 77 //If pilot commanding yaw
joe4465 1:045edcf091f3 78 if(abs(_rcMappedCommands[0]) > 0)
joe4465 0:0010a5abcc31 79 {
joe4465 1:045edcf091f3 80 _stabPIDControllerOutputs[0] = _rcMappedCommands[0]; //Feed to rate PID (overwriting stab PID output)
joe4465 0:0010a5abcc31 81 _yawTarget = _ypr[0];
joe4465 0:0010a5abcc31 82 }
joe4465 0:0010a5abcc31 83
joe4465 0:0010a5abcc31 84 //Update rate PID process value with gyro rate
joe4465 0:0010a5abcc31 85 _yawRatePIDController->setProcessValue(_gyroRate[0]);
joe4465 0:0010a5abcc31 86 _pitchRatePIDController->setProcessValue(_gyroRate[1]);
joe4465 0:0010a5abcc31 87 _rollRatePIDController->setProcessValue(_gyroRate[2]);
joe4465 0:0010a5abcc31 88
joe4465 0:0010a5abcc31 89 //Update rate PID set point with desired rate from stab PID
joe4465 0:0010a5abcc31 90 _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
joe4465 0:0010a5abcc31 91 _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
joe4465 0:0010a5abcc31 92 _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
joe4465 0:0010a5abcc31 93
joe4465 0:0010a5abcc31 94 //Compute rate PID outputs
joe4465 0:0010a5abcc31 95 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 0:0010a5abcc31 96 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 97 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 0:0010a5abcc31 98 }
joe4465 0:0010a5abcc31 99
joe4465 0:0010a5abcc31 100 //Testing
joe4465 1:045edcf091f3 101 _ratePIDControllerOutputs[0] = 0; // yaw
joe4465 0:0010a5abcc31 102 //_ratePIDControllerOutputs[1] = 0; // pitch
joe4465 3:82665e39f1ea 103 //_ratePIDControllerOutputs[2] = 0; // roll
joe4465 3:82665e39f1ea 104 _stabPIDControllerOutputs[0] = 0; // yaw
joe4465 3:82665e39f1ea 105 //_stabPIDControllerOutputs[1] = 0; // pitch
joe4465 3:82665e39f1ea 106 //_stabPIDControllerOutputs[2] = 0; // roll
joe4465 0:0010a5abcc31 107
joe4465 0:0010a5abcc31 108 //Calculate motor power if flying
joe4465 3:82665e39f1ea 109 //RC Mapped thottle is between 0 and 1
joe4465 3:82665e39f1ea 110 //Add 0.1 to try to avoid false starts
joe4465 3:82665e39f1ea 111 if(_rcMappedCommands[3] > (RC_THRUST_MIN + 0.1) && _armed == true)
joe4465 0:0010a5abcc31 112 {
joe4465 3:82665e39f1ea 113 //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
joe4465 3:82665e39f1ea 114 float basePower = MOTORS_MIN + (_rcMappedCommands[3] * 800);
joe4465 3:82665e39f1ea 115
joe4465 3:82665e39f1ea 116 //Map motor power - each PID returns -100 <-> 100
joe4465 3:82665e39f1ea 117 _motorPower[0] = basePower + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 118 _motorPower[1] = basePower + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 119 _motorPower[2] = basePower - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 120 _motorPower[3] = basePower - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
joe4465 0:0010a5abcc31 121
joe4465 3:82665e39f1ea 122 //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
joe4465 3:82665e39f1ea 123 float motorFix = 0;
joe4465 3:82665e39f1ea 124 float motorMin = _motorPower[0];
joe4465 3:82665e39f1ea 125 float motorMax = _motorPower[0];
joe4465 3:82665e39f1ea 126 for(int i=1; i<4; i++)
joe4465 3:82665e39f1ea 127 {
joe4465 3:82665e39f1ea 128 if(_motorPower[i] < motorMin) motorMin = _motorPower[i];
joe4465 3:82665e39f1ea 129 if(_motorPower[i] > motorMax) motorMax = _motorPower[i];
joe4465 3:82665e39f1ea 130 }
joe4465 3:82665e39f1ea 131
joe4465 3:82665e39f1ea 132 //Check if min or max is outside of the limits
joe4465 3:82665e39f1ea 133 if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
joe4465 3:82665e39f1ea 134 else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
joe4465 3:82665e39f1ea 135
joe4465 3:82665e39f1ea 136 //Add/remove constant if neccessary
joe4465 3:82665e39f1ea 137 for(int i=0; i<4; i++)
joe4465 3:82665e39f1ea 138 {
joe4465 3:82665e39f1ea 139 _motorPower[i] = _motorPower[i] + motorFix;
joe4465 3:82665e39f1ea 140 }
joe4465 0:0010a5abcc31 141 }
joe4465 0:0010a5abcc31 142
joe4465 0:0010a5abcc31 143 //Not flying
joe4465 0:0010a5abcc31 144 else if(_armed == true)
joe4465 0:0010a5abcc31 145 {
joe4465 1:045edcf091f3 146 _yawTarget = _ypr[0];
joe4465 1:045edcf091f3 147
joe4465 0:0010a5abcc31 148 //Set motors to armed state
joe4465 0:0010a5abcc31 149 _motorPower[0] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 150 _motorPower[1] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 151 _motorPower[2] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 152 _motorPower[3] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 153
joe4465 0:0010a5abcc31 154 _notFlying ++;
joe4465 0:0010a5abcc31 155 if(_notFlying > 500) //Not flying for 1 second
joe4465 0:0010a5abcc31 156 {
joe4465 3:82665e39f1ea 157 NotFlying();
joe4465 0:0010a5abcc31 158 }
joe4465 0:0010a5abcc31 159 }
joe4465 0:0010a5abcc31 160 else
joe4465 0:0010a5abcc31 161 {
joe4465 0:0010a5abcc31 162 //Disable Motors
joe4465 0:0010a5abcc31 163 _motorPower[0] = MOTORS_OFF;
joe4465 0:0010a5abcc31 164 _motorPower[1] = MOTORS_OFF;
joe4465 0:0010a5abcc31 165 _motorPower[2] = MOTORS_OFF;
joe4465 0:0010a5abcc31 166 _motorPower[3] = MOTORS_OFF;
joe4465 3:82665e39f1ea 167
joe4465 3:82665e39f1ea 168 _notFlying ++;
joe4465 3:82665e39f1ea 169 if(_notFlying > 500) //Not flying for 1 second
joe4465 3:82665e39f1ea 170 {
joe4465 3:82665e39f1ea 171 NotFlying();
joe4465 3:82665e39f1ea 172 }
joe4465 0:0010a5abcc31 173 }
joe4465 0:0010a5abcc31 174
joe4465 0:0010a5abcc31 175 //Set motor power
joe4465 0:0010a5abcc31 176 _motor1.pulsewidth_us(_motorPower[0]);
joe4465 0:0010a5abcc31 177 _motor2.pulsewidth_us(_motorPower[1]);
joe4465 0:0010a5abcc31 178 _motor3.pulsewidth_us(_motorPower[2]);
joe4465 0:0010a5abcc31 179 _motor4.pulsewidth_us(_motorPower[3]);
joe4465 0:0010a5abcc31 180 }
joe4465 0:0010a5abcc31 181
joe4465 3:82665e39f1ea 182 void GetAttitude()
joe4465 0:0010a5abcc31 183 {
joe4465 3:82665e39f1ea 184 //Get raw data from IMU
joe4465 5:7b7db24ef6eb 185 _freeIMU.getYawPitchRoll(_ypr); // try get euler to get full rotation in each axis
joe4465 3:82665e39f1ea 186 _freeIMU.getRate(_gyroRate);
joe4465 3:82665e39f1ea 187
joe4465 3:82665e39f1ea 188 //Take off zero values to account for any angle inbetween the PCB level and ground
joe4465 3:82665e39f1ea 189 _ypr[1] = _ypr[1] - _zeroValues[1];
joe4465 3:82665e39f1ea 190 _ypr[2] = _ypr[2] - _zeroValues[2];
joe4465 3:82665e39f1ea 191
joe4465 3:82665e39f1ea 192 //Swap pitch and roll angle because IMU is mounted at a right angle to the board
joe4465 3:82665e39f1ea 193 float pitch = _ypr[2];
joe4465 3:82665e39f1ea 194 float roll = -_ypr[1]; //Needs to be negative
joe4465 3:82665e39f1ea 195 _ypr[1] = pitch;
joe4465 3:82665e39f1ea 196 _ypr[2] = roll;
joe4465 3:82665e39f1ea 197
joe4465 5:7b7db24ef6eb 198 //Swap pitch and roll rate because IMU is mounted at a right angle to the board
joe4465 5:7b7db24ef6eb 199 pitch = _gyroRate[2];
joe4465 5:7b7db24ef6eb 200 roll = -_gyroRate[1];
joe4465 5:7b7db24ef6eb 201 _gyroRate[1] = pitch;
joe4465 5:7b7db24ef6eb 202 _gyroRate[2] = roll;
joe4465 5:7b7db24ef6eb 203
joe4465 3:82665e39f1ea 204 //Try swapping yaw
joe4465 3:82665e39f1ea 205 //_ypr[0] = - _ypr[0];
joe4465 3:82665e39f1ea 206 }
joe4465 3:82665e39f1ea 207
joe4465 3:82665e39f1ea 208 void NotFlying()
joe4465 3:82665e39f1ea 209 {
joe4465 3:82665e39f1ea 210 //Reset iteratior
joe4465 3:82665e39f1ea 211 _notFlying = 0;
joe4465 3:82665e39f1ea 212
joe4465 3:82665e39f1ea 213 //Zero gyro
joe4465 3:82665e39f1ea 214 _freeIMU.zeroGyro();
joe4465 3:82665e39f1ea 215
joe4465 3:82665e39f1ea 216 //Reset I
joe4465 3:82665e39f1ea 217 _yawRatePIDController->reset();
joe4465 3:82665e39f1ea 218 _pitchRatePIDController->reset();
joe4465 3:82665e39f1ea 219 _rollRatePIDController->reset();
joe4465 3:82665e39f1ea 220 _yawStabPIDController->reset();
joe4465 3:82665e39f1ea 221 _pitchStabPIDController->reset();
joe4465 3:82665e39f1ea 222 _rollStabPIDController->reset();
joe4465 0:0010a5abcc31 223 }