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@2:b3b771c8f7d1, 2014-05-16 (annotated)

Committer:
joe4465
Date:
Fri May 16 14:22:18 2014 +0000
Revision:
2:b3b771c8f7d1
Parent:
1:045edcf091f3
Child:
3:82665e39f1ea
second commit

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 float Constrain(const float in, const float min, const float max);
joe4465 0:0010a5abcc31 7 float map(float x, float in_min, float in_max, float out_min, float out_max);
joe4465 0:0010a5abcc31 8 void GetAttitude();
joe4465 0:0010a5abcc31 9 void Task500Hz(void const *n);
joe4465 0:0010a5abcc31 10 void Task10Hz();
joe4465 0:0010a5abcc31 11
joe4465 0:0010a5abcc31 12 //Variables
joe4465 0:0010a5abcc31 13 float _gyroRate[3] ={}; // Yaw, Pitch, Roll
joe4465 2:b3b771c8f7d1 14 float _ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 0:0010a5abcc31 15 float _yawTarget = 0;
joe4465 0:0010a5abcc31 16 int _notFlying = 0;
joe4465 0:0010a5abcc31 17 float _altitude = 0;
joe4465 0:0010a5abcc31 18 int _10HzIterator = 0;
joe4465 2:b3b771c8f7d1 19 float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
joe4465 2:b3b771c8f7d1 20 float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
joe4465 2:b3b771c8f7d1 21 float _motorPower [4] = {0,0,0,0};
joe4465 2:b3b771c8f7d1 22
joe4465 0:0010a5abcc31 23
joe4465 1:045edcf091f3 24 //Timers
joe4465 1:045edcf091f3 25 RtosTimer *_updateTimer;
joe4465 1:045edcf091f3 26
joe4465 0:0010a5abcc31 27 // A thread to monitor the serial ports
joe4465 0:0010a5abcc31 28 void FlightControllerThread(void const *args)
joe4465 0:0010a5abcc31 29 {
joe4465 1:045edcf091f3 30 //Update Timer
joe4465 0:0010a5abcc31 31 _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
joe4465 0:0010a5abcc31 32 int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
joe4465 0:0010a5abcc31 33 _updateTimer->start(updateTime);
joe4465 0:0010a5abcc31 34
joe4465 0:0010a5abcc31 35 // Wait here forever
joe4465 0:0010a5abcc31 36 Thread::wait(osWaitForever);
joe4465 0:0010a5abcc31 37 }
joe4465 0:0010a5abcc31 38
joe4465 0:0010a5abcc31 39 //Constrains value to between min and max
joe4465 0:0010a5abcc31 40 float Constrain(const float in, const float min, const float max)
joe4465 0:0010a5abcc31 41 {
joe4465 0:0010a5abcc31 42 float out = in;
joe4465 0:0010a5abcc31 43 out = out > max ? max : out;
joe4465 0:0010a5abcc31 44 out = out < min ? min : out;
joe4465 0:0010a5abcc31 45 return out;
joe4465 0:0010a5abcc31 46 }
joe4465 0:0010a5abcc31 47
joe4465 0:0010a5abcc31 48 //Maps input to output
joe4465 0:0010a5abcc31 49 float map(float x, float in_min, float in_max, float out_min, float out_max)
joe4465 0:0010a5abcc31 50 {
joe4465 0:0010a5abcc31 51 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
joe4465 0:0010a5abcc31 52 }
joe4465 0:0010a5abcc31 53
joe4465 0:0010a5abcc31 54 //Zeros values
joe4465 0:0010a5abcc31 55 void GetAttitude()
joe4465 0:0010a5abcc31 56 {
joe4465 1:045edcf091f3 57 //Take off zero values to account for any angle inbetween the PCB level and ground
joe4465 0:0010a5abcc31 58 _ypr[1] = _ypr[1] - _zeroValues[1];
joe4465 0:0010a5abcc31 59 _ypr[2] = _ypr[2] - _zeroValues[2];
joe4465 0:0010a5abcc31 60
joe4465 0:0010a5abcc31 61 //Swap pitch and roll because IMU is mounted at a right angle to the board
joe4465 0:0010a5abcc31 62 float pitch = _ypr[2];
joe4465 0:0010a5abcc31 63 float roll = _ypr[1];
joe4465 0:0010a5abcc31 64 _ypr[1] = pitch;
joe4465 0:0010a5abcc31 65 _ypr[2] = roll;
joe4465 0:0010a5abcc31 66 }
joe4465 0:0010a5abcc31 67
joe4465 0:0010a5abcc31 68 void Task500Hz(void const *n)
joe4465 0:0010a5abcc31 69 {
joe4465 1:045edcf091f3 70 _10HzIterator++;
joe4465 1:045edcf091f3 71 if(_10HzIterator % 50 == 0)
joe4465 0:0010a5abcc31 72 {
joe4465 1:045edcf091f3 73 Task10Hz();
joe4465 0:0010a5abcc31 74 }
joe4465 0:0010a5abcc31 75
joe4465 0:0010a5abcc31 76 //Get IMU data
joe4465 0:0010a5abcc31 77 _freeIMU.getYawPitchRoll(_ypr);
joe4465 0:0010a5abcc31 78 _freeIMU.getRate(_gyroRate);
joe4465 0:0010a5abcc31 79 GetAttitude();
joe4465 0:0010a5abcc31 80
joe4465 0:0010a5abcc31 81 //Rate mode
joe4465 0:0010a5abcc31 82 if(_rate == true && _stab == false)
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 RC
joe4465 1:045edcf091f3 90 _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
joe4465 1:045edcf091f3 91 _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 92 _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 93
joe4465 0:0010a5abcc31 94 //Compute rate PID outputs
joe4465 0:0010a5abcc31 95 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 1:045edcf091f3 96 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 97 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 0:0010a5abcc31 98 }
joe4465 0:0010a5abcc31 99 //Stability mode
joe4465 0:0010a5abcc31 100 else
joe4465 0:0010a5abcc31 101 {
joe4465 0:0010a5abcc31 102 //Update stab PID process value with ypr
joe4465 0:0010a5abcc31 103 _yawStabPIDController->setProcessValue(_ypr[0]);
joe4465 0:0010a5abcc31 104 _pitchStabPIDController->setProcessValue(_ypr[1]);
joe4465 0:0010a5abcc31 105 _rollStabPIDController->setProcessValue(_ypr[2]);
joe4465 0:0010a5abcc31 106
joe4465 0:0010a5abcc31 107 //Update stab PID set point with desired angle from RC
joe4465 0:0010a5abcc31 108 _yawStabPIDController->setSetPoint(_yawTarget);
joe4465 1:045edcf091f3 109 _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
joe4465 1:045edcf091f3 110 _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
joe4465 0:0010a5abcc31 111
joe4465 0:0010a5abcc31 112 //Compute stab PID outputs
joe4465 0:0010a5abcc31 113 _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
joe4465 1:045edcf091f3 114 _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
joe4465 0:0010a5abcc31 115 _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
joe4465 0:0010a5abcc31 116
joe4465 1:045edcf091f3 117 //If pilot commanding yaw
joe4465 1:045edcf091f3 118 if(abs(_rcMappedCommands[0]) > 0)
joe4465 0:0010a5abcc31 119 {
joe4465 1:045edcf091f3 120 _stabPIDControllerOutputs[0] = _rcMappedCommands[0]; //Feed to rate PID (overwriting stab PID output)
joe4465 0:0010a5abcc31 121 _yawTarget = _ypr[0];
joe4465 0:0010a5abcc31 122 }
joe4465 0:0010a5abcc31 123
joe4465 0:0010a5abcc31 124 //Update rate PID process value with gyro rate
joe4465 0:0010a5abcc31 125 _yawRatePIDController->setProcessValue(_gyroRate[0]);
joe4465 0:0010a5abcc31 126 _pitchRatePIDController->setProcessValue(_gyroRate[1]);
joe4465 0:0010a5abcc31 127 _rollRatePIDController->setProcessValue(_gyroRate[2]);
joe4465 0:0010a5abcc31 128
joe4465 0:0010a5abcc31 129 //Update rate PID set point with desired rate from stab PID
joe4465 0:0010a5abcc31 130 _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
joe4465 0:0010a5abcc31 131 _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
joe4465 0:0010a5abcc31 132 _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
joe4465 0:0010a5abcc31 133
joe4465 0:0010a5abcc31 134 //Compute rate PID outputs
joe4465 0:0010a5abcc31 135 _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
joe4465 0:0010a5abcc31 136 _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
joe4465 0:0010a5abcc31 137 _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
joe4465 0:0010a5abcc31 138 }
joe4465 0:0010a5abcc31 139
joe4465 0:0010a5abcc31 140 //Testing
joe4465 1:045edcf091f3 141 _ratePIDControllerOutputs[0] = 0; // yaw
joe4465 0:0010a5abcc31 142 //_ratePIDControllerOutputs[1] = 0; // pitch
joe4465 1:045edcf091f3 143 _ratePIDControllerOutputs[2] = 0; // roll
joe4465 0:0010a5abcc31 144
joe4465 0:0010a5abcc31 145 //Calculate motor power if flying
joe4465 1:045edcf091f3 146 if(_rcMappedCommands[3] > 0.1 && _armed == true)
joe4465 0:0010a5abcc31 147 {
joe4465 0:0010a5abcc31 148 //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
joe4465 1:045edcf091f3 149 _motorPower[0] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
joe4465 1:045edcf091f3 150 _motorPower[1] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
joe4465 1:045edcf091f3 151 _motorPower[2] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
joe4465 1:045edcf091f3 152 _motorPower[3] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
joe4465 0:0010a5abcc31 153
joe4465 0:0010a5abcc31 154
joe4465 0:0010a5abcc31 155 //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
joe4465 1:045edcf091f3 156 _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1500); //Reduced to 1500 to limit power for testing
joe4465 1:045edcf091f3 157 _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1500);
joe4465 1:045edcf091f3 158 _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1500);
joe4465 1:045edcf091f3 159 _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1500);
joe4465 0:0010a5abcc31 160 }
joe4465 0:0010a5abcc31 161
joe4465 0:0010a5abcc31 162 //Not flying
joe4465 0:0010a5abcc31 163 else if(_armed == true)
joe4465 0:0010a5abcc31 164 {
joe4465 1:045edcf091f3 165 _yawTarget = _ypr[0];
joe4465 1:045edcf091f3 166
joe4465 0:0010a5abcc31 167 //Set motors to armed state
joe4465 0:0010a5abcc31 168 _motorPower[0] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 169 _motorPower[1] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 170 _motorPower[2] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 171 _motorPower[3] = MOTORS_ARMED;
joe4465 0:0010a5abcc31 172
joe4465 0:0010a5abcc31 173 _notFlying ++;
joe4465 0:0010a5abcc31 174 if(_notFlying > 500) //Not flying for 1 second
joe4465 0:0010a5abcc31 175 {
joe4465 0:0010a5abcc31 176 //Reset iteratior
joe4465 0:0010a5abcc31 177 _notFlying = 0;
joe4465 0:0010a5abcc31 178
joe4465 0:0010a5abcc31 179 //Reset I
joe4465 0:0010a5abcc31 180 _yawRatePIDController->reset();
joe4465 0:0010a5abcc31 181 _pitchRatePIDController->reset();
joe4465 0:0010a5abcc31 182 _rollRatePIDController->reset();
joe4465 0:0010a5abcc31 183 _yawStabPIDController->reset();
joe4465 0:0010a5abcc31 184 _pitchStabPIDController->reset();
joe4465 0:0010a5abcc31 185 _rollStabPIDController->reset();
joe4465 0:0010a5abcc31 186 }
joe4465 0:0010a5abcc31 187 }
joe4465 0:0010a5abcc31 188 else
joe4465 0:0010a5abcc31 189 {
joe4465 0:0010a5abcc31 190 //Disable Motors
joe4465 0:0010a5abcc31 191 _motorPower[0] = MOTORS_OFF;
joe4465 0:0010a5abcc31 192 _motorPower[1] = MOTORS_OFF;
joe4465 0:0010a5abcc31 193 _motorPower[2] = MOTORS_OFF;
joe4465 0:0010a5abcc31 194 _motorPower[3] = MOTORS_OFF;
joe4465 0:0010a5abcc31 195 }
joe4465 0:0010a5abcc31 196
joe4465 0:0010a5abcc31 197 //Set motor power
joe4465 0:0010a5abcc31 198 _motor1.pulsewidth_us(_motorPower[0]);
joe4465 0:0010a5abcc31 199 _motor2.pulsewidth_us(_motorPower[1]);
joe4465 0:0010a5abcc31 200 _motor3.pulsewidth_us(_motorPower[2]);
joe4465 0:0010a5abcc31 201 _motor4.pulsewidth_us(_motorPower[3]);
joe4465 0:0010a5abcc31 202 }
joe4465 0:0010a5abcc31 203
joe4465 0:0010a5abcc31 204 //Print data
joe4465 0:0010a5abcc31 205 void Task10Hz()
joe4465 0:0010a5abcc31 206 {
joe4465 0:0010a5abcc31 207 int batt = 0;
joe4465 0:0010a5abcc31 208 _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f>",
joe4465 1:045edcf091f3 209 _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 0:0010a5abcc31 210 }