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