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@7:bc5822aa8878, 2015-02-10 (annotated)

Committer:
joe4465
Date:
Tue Feb 10 20:55:44 2015 +0000
Revision:
7:bc5822aa8878
Parent:
6:4c207e7b1203
Child:
9:7b194f83e567
Updated software to match new PCB

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