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@9:7b194f83e567, 2015-02-22 (annotated)

Committer:
joe4465
Date:
Sun Feb 22 20:10:12 2015 +0000
Revision:
9:7b194f83e567
Parent:
7:bc5822aa8878 
Added external magnetometer

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 int _notFlying = 0;
joe4465 0:0010a5abcc31 12
joe4465 1:045edcf091f3 13 //Timers
joe4465 3:82665e39f1ea 14 RtosTimer *_flightControllerUpdateTimer;
joe4465 1:045edcf091f3 15
joe4465 3:82665e39f1ea 16 // A thread to control flight
joe4465 0:0010a5abcc31 17 void FlightControllerThread(void const *args)
joe4465 6:4c207e7b1203 18 {
joe4465 6:4c207e7b1203 19 printf("Flight controller thread started\r\n");
joe4465 6:4c207e7b1203 20
joe4465 1:045edcf091f3 21 //Update Timer
joe4465 3:82665e39f1ea 22 _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
joe4465 3:82665e39f1ea 23 int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
joe4465 3:82665e39f1ea 24 _flightControllerUpdateTimer->start(updateTime);
joe4465 0:0010a5abcc31 25
joe4465 0:0010a5abcc31 26 // Wait here forever
joe4465 0:0010a5abcc31 27 Thread::wait(osWaitForever);
joe4465 0:0010a5abcc31 28 }
joe4465 0:0010a5abcc31 29
joe4465 3:82665e39f1ea 30 void FlightControllerTask(void const *n)
joe4465 0:0010a5abcc31 31 {
joe4465 0:0010a5abcc31 32 //Get IMU data
joe4465 0:0010a5abcc31 33 GetAttitude();
joe4465 0:0010a5abcc31 34
joe4465 0:0010a5abcc31 35 //Rate mode
joe4465 0:0010a5abcc31 36 if(_rate == true && _stab == false)
joe4465 0:0010a5abcc31 37 {
joe4465 0:0010a5abcc31 38 //Update rate PID process value with gyro rate
joe4465 0:0010a5abcc31 39 _yawRatePIDController->setProcessValue(_gyroRate[0]);
joe4465 0:0010a5abcc31 40 _pitchRatePIDController->setProcessValue(_gyroRate[1]);
joe4465 0:0010a5abcc31 41 _rollRatePIDController->setProcessValue(_gyroRate[2]);
joe4465 0:0010a5abcc31 42
joe4465 0:0010a5abcc31 43 //Update rate PID set point with desired rate from RC
joe4465 1:045edcf091f3 44 _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
joe4465 1:045edcf091f3 45 _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 46 _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 47
joe4465 0:0010a5abcc31 48 //Compute rate PID outputs
joe4465 0:0010a5abcc31 49 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 1:045edcf091f3 50 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 51 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 3:82665e39f1ea 52
joe4465 3:82665e39f1ea 53 //Set stability PID outputs to 0
joe4465 3:82665e39f1ea 54 _stabPIDControllerOutputs[0] = 0;
joe4465 3:82665e39f1ea 55 _stabPIDControllerOutputs[1] = 0;
joe4465 3:82665e39f1ea 56 _stabPIDControllerOutputs[2] = 0;
joe4465 0:0010a5abcc31 57 }
joe4465 0:0010a5abcc31 58 //Stability mode
joe4465 3:82665e39f1ea 59 else if(_rate == false && _stab == true)
joe4465 0:0010a5abcc31 60 {
joe4465 0:0010a5abcc31 61 //Update stab PID process value with ypr
joe4465 0:0010a5abcc31 62 _yawStabPIDController->setProcessValue(_ypr[0]);
joe4465 0:0010a5abcc31 63 _pitchStabPIDController->setProcessValue(_ypr[1]);
joe4465 0:0010a5abcc31 64 _rollStabPIDController->setProcessValue(_ypr[2]);
joe4465 0:0010a5abcc31 65
joe4465 0:0010a5abcc31 66 //Update stab PID set point with desired angle from RC
joe4465 0:0010a5abcc31 67 _yawStabPIDController->setSetPoint(_yawTarget);
joe4465 1:045edcf091f3 68 _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 69 _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 70
joe4465 0:0010a5abcc31 71 //Compute stab PID outputs
joe4465 0:0010a5abcc31 72 _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
joe4465 1:045edcf091f3 73 _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
joe4465 0:0010a5abcc31 74 _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
joe4465 0:0010a5abcc31 75
joe4465 1:045edcf091f3 76 //If pilot commanding yaw
joe4465 1:045edcf091f3 77 if(abs(_rcMappedCommands[0]) > 0)
joe4465 0:0010a5abcc31 78 {
joe4465 1:045edcf091f3 79 _stabPIDControllerOutputs[0] = _rcMappedCommands[0]; //Feed to rate PID (overwriting stab PID output)
joe4465 0:0010a5abcc31 80 _yawTarget = _ypr[0];
joe4465 0:0010a5abcc31 81 }
joe4465 0:0010a5abcc31 82
joe4465 0:0010a5abcc31 83 //Update rate PID process value with gyro rate
joe4465 0:0010a5abcc31 84 _yawRatePIDController->setProcessValue(_gyroRate[0]);
joe4465 0:0010a5abcc31 85 _pitchRatePIDController->setProcessValue(_gyroRate[1]);
joe4465 0:0010a5abcc31 86 _rollRatePIDController->setProcessValue(_gyroRate[2]);
joe4465 0:0010a5abcc31 87
joe4465 0:0010a5abcc31 88 //Update rate PID set point with desired rate from stab PID
joe4465 0:0010a5abcc31 89 _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
joe4465 0:0010a5abcc31 90 _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
joe4465 0:0010a5abcc31 91 _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
joe4465 0:0010a5abcc31 92
joe4465 0:0010a5abcc31 93 //Compute rate PID outputs
joe4465 0:0010a5abcc31 94 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 0:0010a5abcc31 95 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 96 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 0:0010a5abcc31 97 }
joe4465 0:0010a5abcc31 98
joe4465 0:0010a5abcc31 99 //Testing
joe4465 6:4c207e7b1203 100 //_ratePIDControllerOutputs[0] = 0; // yaw
joe4465 0:0010a5abcc31 101 //_ratePIDControllerOutputs[1] = 0; // pitch
joe4465 3:82665e39f1ea 102 //_ratePIDControllerOutputs[2] = 0; // roll
joe4465 6:4c207e7b1203 103 //_stabPIDControllerOutputs[0] = 0; // yaw
joe4465 3:82665e39f1ea 104 //_stabPIDControllerOutputs[1] = 0; // pitch
joe4465 3:82665e39f1ea 105 //_stabPIDControllerOutputs[2] = 0; // roll
joe4465 0:0010a5abcc31 106
joe4465 0:0010a5abcc31 107 //Calculate motor power if flying
joe4465 3:82665e39f1ea 108 //RC Mapped thottle is between 0 and 1
joe4465 7:bc5822aa8878 109 //Add 0.2 to try to avoid false starts
joe4465 7:bc5822aa8878 110 if(_rcMappedCommands[3] > (RC_THRUST_MIN + 0.2) && _armed == true)
joe4465 0:0010a5abcc31 111 {
joe4465 3:82665e39f1ea 112 //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
joe4465 3:82665e39f1ea 113 float basePower = MOTORS_MIN + (_rcMappedCommands[3] * 800);
joe4465 3:82665e39f1ea 114
joe4465 3:82665e39f1ea 115 //Map motor power - each PID returns -100 <-> 100
joe4465 3:82665e39f1ea 116 _motorPower[0] = basePower + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 117 _motorPower[1] = basePower + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 118 _motorPower[2] = basePower - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
joe4465 3:82665e39f1ea 119 _motorPower[3] = basePower - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
joe4465 0:0010a5abcc31 120
joe4465 3:82665e39f1ea 121 //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
joe4465 3:82665e39f1ea 122 float motorFix = 0;
joe4465 3:82665e39f1ea 123 float motorMin = _motorPower[0];
joe4465 3:82665e39f1ea 124 float motorMax = _motorPower[0];
joe4465 7:bc5822aa8878 125
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 6:4c207e7b1203 184
joe4465 3:82665e39f1ea 185 //Get raw data from IMU
joe4465 6:4c207e7b1203 186 _freeIMU.getYawPitchRoll(_ypr);
joe4465 3:82665e39f1ea 187 _freeIMU.getRate(_gyroRate);
joe4465 3:82665e39f1ea 188
joe4465 9:7b194f83e567 189 //Take off zero values to account for any angle inbetween level and ground
joe4465 9:7b194f83e567 190 //_ypr[1] = _ypr[1] - _zeroValues[1];
joe4465 9:7b194f83e567 191 //_ypr[2] = _ypr[2] - _zeroValues[2];
joe4465 3:82665e39f1ea 192
joe4465 3:82665e39f1ea 193 //Swap pitch and roll angle because IMU is mounted at a right angle to the board
joe4465 3:82665e39f1ea 194 float pitch = _ypr[2];
joe4465 9:7b194f83e567 195 float roll = -_ypr[1];
joe4465 3:82665e39f1ea 196 _ypr[1] = pitch;
joe4465 3:82665e39f1ea 197 _ypr[2] = roll;
joe4465 3:82665e39f1ea 198
joe4465 9:7b194f83e567 199 _ypr[0] = _ypr[0];
joe4465 9:7b194f83e567 200
joe4465 9:7b194f83e567 201 //Swap pitch, roll and yaw rate because IMU is mounted at a right angle to the board
joe4465 9:7b194f83e567 202 float yaw = _gyroRate[2];
joe4465 6:4c207e7b1203 203 pitch = _gyroRate[0];
joe4465 6:4c207e7b1203 204 roll = _gyroRate[1];
joe4465 6:4c207e7b1203 205 _gyroRate[0] = yaw;
joe4465 5:7b7db24ef6eb 206 _gyroRate[1] = pitch;
joe4465 5:7b7db24ef6eb 207 _gyroRate[2] = roll;
joe4465 3:82665e39f1ea 208 }
joe4465 3:82665e39f1ea 209
joe4465 3:82665e39f1ea 210 void NotFlying()
joe4465 3:82665e39f1ea 211 {
joe4465 3:82665e39f1ea 212 //Reset iteratior
joe4465 3:82665e39f1ea 213 _notFlying = 0;
joe4465 3:82665e39f1ea 214
joe4465 3:82665e39f1ea 215 //Zero gyro
joe4465 3:82665e39f1ea 216 _freeIMU.zeroGyro();
joe4465 3:82665e39f1ea 217
joe4465 3:82665e39f1ea 218 //Reset I
joe4465 3:82665e39f1ea 219 _yawRatePIDController->reset();
joe4465 3:82665e39f1ea 220 _pitchRatePIDController->reset();
joe4465 3:82665e39f1ea 221 _rollRatePIDController->reset();
joe4465 3:82665e39f1ea 222 _yawStabPIDController->reset();
joe4465 3:82665e39f1ea 223 _pitchStabPIDController->reset();
joe4465 3:82665e39f1ea 224 _rollStabPIDController->reset();
joe4465 0:0010a5abcc31 225 }