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@0:0010a5abcc31, 2014-05-09 (annotated)

Committer:
joe4465
Date:
Fri May 09 10:04:36 2014 +0000
Revision:
0:0010a5abcc31
Child:
1:045edcf091f3
Added get rate function that returns the gyroscope rate - yaw, pitch, roll

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