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

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 //Includes
joe4465 0:0010a5abcc31 2 #include "mbed.h"
joe4465 0:0010a5abcc31 3 #include "rtos.h"
joe4465 0:0010a5abcc31 4 #include "FreeIMU.h"
joe4465 0:0010a5abcc31 5 #include "PID.h"
joe4465 0:0010a5abcc31 6 #include "ConfigFile.h"
joe4465 0:0010a5abcc31 7 #include "hardware.h"
joe4465 0:0010a5abcc31 8 #include "statusLights.h"
joe4465 0:0010a5abcc31 9 #include "serialPortMonitor.h"
joe4465 0:0010a5abcc31 10 #include "flightController.h"
joe4465 3:82665e39f1ea 11 #include "rcCommandMonitor.h"
joe4465 9:7b194f83e567 12 #include "altitudeMonitor.h"
joe4465 0:0010a5abcc31 13
joe4465 0:0010a5abcc31 14 //Declarations
joe4465 0:0010a5abcc31 15 void LoadSettingsFromConfig();
joe4465 0:0010a5abcc31 16 void InitialisePID();
joe4465 0:0010a5abcc31 17 void InitialisePWM();
joe4465 0:0010a5abcc31 18 void Setup();
joe4465 0:0010a5abcc31 19
joe4465 6:4c207e7b1203 20 //Loads settings from the config file - could tidy this a little if I can be assed
joe4465 0:0010a5abcc31 21 void LoadSettingsFromConfig()
joe4465 0:0010a5abcc31 22 {
joe4465 6:4c207e7b1203 23 _wiredSerial.printf("Starting to load settings from config file\n\r");
joe4465 0:0010a5abcc31 24
joe4465 0:0010a5abcc31 25 //_wiredSerial.printf("Loading settings from config file\n\r");
joe4465 0:0010a5abcc31 26 char value[BUFSIZ];
joe4465 0:0010a5abcc31 27
joe4465 0:0010a5abcc31 28 //Read a configuration file from a mbed.
joe4465 0:0010a5abcc31 29 if (!_configFile.read("/local/config.cfg"))
joe4465 0:0010a5abcc31 30 {
joe4465 0:0010a5abcc31 31 _wiredSerial.printf("Config file does not exist\n\r");
joe4465 0:0010a5abcc31 32 }
joe4465 0:0010a5abcc31 33 else
joe4465 6:4c207e7b1203 34 {
joe4465 6:4c207e7b1203 35 //Get values
joe4465 6:4c207e7b1203 36 if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
joe4465 0:0010a5abcc31 37 else
joe4465 6:4c207e7b1203 38 {
joe4465 6:4c207e7b1203 39 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 40 }
joe4465 6:4c207e7b1203 41 if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
joe4465 6:4c207e7b1203 42 else
joe4465 6:4c207e7b1203 43 {
joe4465 6:4c207e7b1203 44 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 45 }
joe4465 6:4c207e7b1203 46 if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
joe4465 6:4c207e7b1203 47 else
joe4465 6:4c207e7b1203 48 {
joe4465 6:4c207e7b1203 49 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 50 }
joe4465 6:4c207e7b1203 51 if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
joe4465 6:4c207e7b1203 52 else
joe4465 6:4c207e7b1203 53 {
joe4465 6:4c207e7b1203 54 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 55 }
joe4465 6:4c207e7b1203 56 if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
joe4465 6:4c207e7b1203 57 else
joe4465 6:4c207e7b1203 58 {
joe4465 6:4c207e7b1203 59 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 60 }
joe4465 6:4c207e7b1203 61 if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
joe4465 6:4c207e7b1203 62 else
joe4465 6:4c207e7b1203 63 {
joe4465 6:4c207e7b1203 64 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 65 }
joe4465 6:4c207e7b1203 66 if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
joe4465 6:4c207e7b1203 67 else
joe4465 6:4c207e7b1203 68 {
joe4465 6:4c207e7b1203 69 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 70 }
joe4465 6:4c207e7b1203 71 if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
joe4465 6:4c207e7b1203 72 else
joe4465 6:4c207e7b1203 73 {
joe4465 6:4c207e7b1203 74 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 75 }
joe4465 6:4c207e7b1203 76 if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
joe4465 6:4c207e7b1203 77 else
joe4465 6:4c207e7b1203 78 {
joe4465 6:4c207e7b1203 79 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 80 }
joe4465 6:4c207e7b1203 81
joe4465 6:4c207e7b1203 82 if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
joe4465 6:4c207e7b1203 83 else
joe4465 6:4c207e7b1203 84 {
joe4465 6:4c207e7b1203 85 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 86 }
joe4465 6:4c207e7b1203 87 if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
joe4465 6:4c207e7b1203 88 else
joe4465 6:4c207e7b1203 89 {
joe4465 6:4c207e7b1203 90 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 91 }
joe4465 6:4c207e7b1203 92 if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
joe4465 6:4c207e7b1203 93 else
joe4465 6:4c207e7b1203 94 {
joe4465 6:4c207e7b1203 95 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 96 }
joe4465 6:4c207e7b1203 97 if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
joe4465 6:4c207e7b1203 98 else
joe4465 6:4c207e7b1203 99 {
joe4465 6:4c207e7b1203 100 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 101 }
joe4465 6:4c207e7b1203 102 if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
joe4465 6:4c207e7b1203 103 else
joe4465 6:4c207e7b1203 104 {
joe4465 6:4c207e7b1203 105 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 106 }
joe4465 6:4c207e7b1203 107 if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
joe4465 6:4c207e7b1203 108 else
joe4465 6:4c207e7b1203 109 {
joe4465 6:4c207e7b1203 110 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 111 }
joe4465 6:4c207e7b1203 112 if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
joe4465 6:4c207e7b1203 113 else
joe4465 6:4c207e7b1203 114 {
joe4465 6:4c207e7b1203 115 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 116 }
joe4465 6:4c207e7b1203 117 if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
joe4465 6:4c207e7b1203 118 else
joe4465 6:4c207e7b1203 119 {
joe4465 6:4c207e7b1203 120 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 121 }
joe4465 6:4c207e7b1203 122 if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
joe4465 6:4c207e7b1203 123 else
joe4465 6:4c207e7b1203 124 {
joe4465 6:4c207e7b1203 125 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 126 }
joe4465 6:4c207e7b1203 127 if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
joe4465 6:4c207e7b1203 128 else
joe4465 6:4c207e7b1203 129 {
joe4465 6:4c207e7b1203 130 _wiredSerial.printf("Failed to get value for zero pitch\n\r");
joe4465 6:4c207e7b1203 131 }
joe4465 6:4c207e7b1203 132 if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
joe4465 6:4c207e7b1203 133 else
joe4465 6:4c207e7b1203 134 {
joe4465 6:4c207e7b1203 135 _wiredSerial.printf("Failed to get value for zero roll\n\r");
joe4465 6:4c207e7b1203 136 }
joe4465 0:0010a5abcc31 137 }
joe4465 0:0010a5abcc31 138
joe4465 6:4c207e7b1203 139 _wiredSerial.printf("Finished loading settings from config file\n\r");
joe4465 0:0010a5abcc31 140 }
joe4465 0:0010a5abcc31 141
joe4465 0:0010a5abcc31 142 //PID initialisation
joe4465 0:0010a5abcc31 143 void InitialisePID()
joe4465 0:0010a5abcc31 144 {
joe4465 3:82665e39f1ea 145 float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
joe4465 0:0010a5abcc31 146
joe4465 0:0010a5abcc31 147 _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 148 _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
joe4465 0:0010a5abcc31 149 _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 150 _yawRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 151 _yawRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 152 _yawRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 153
joe4465 0:0010a5abcc31 154 _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 155 _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
joe4465 0:0010a5abcc31 156 _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 157 _pitchRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 158 _pitchRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 159 _pitchRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 160
joe4465 0:0010a5abcc31 161 _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 162 _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
joe4465 0:0010a5abcc31 163 _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 164 _rollRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 165 _rollRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 166 _rollRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 167
joe4465 0:0010a5abcc31 168 _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 169 _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX);
joe4465 0:0010a5abcc31 170 _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
joe4465 0:0010a5abcc31 171 _yawStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 172 _yawStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 173 _yawStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 174
joe4465 0:0010a5abcc31 175 _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 176 _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX);
joe4465 0:0010a5abcc31 177 _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
joe4465 0:0010a5abcc31 178 _pitchStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 179 _pitchStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 180 _pitchStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 181
joe4465 0:0010a5abcc31 182 _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 183 _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX);
joe4465 0:0010a5abcc31 184 _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
joe4465 0:0010a5abcc31 185 _rollStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 186 _rollStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 187 _rollStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 188 }
joe4465 0:0010a5abcc31 189
joe4465 0:0010a5abcc31 190 //PWM Initialisation
joe4465 0:0010a5abcc31 191 void InitialisePWM()
joe4465 0:0010a5abcc31 192 {
joe4465 1:045edcf091f3 193 //500Hz
joe4465 6:4c207e7b1203 194 float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
joe4465 0:0010a5abcc31 195 _motor1.period(period);
joe4465 0:0010a5abcc31 196 _motor2.period(period);
joe4465 0:0010a5abcc31 197 _motor3.period(period);
joe4465 0:0010a5abcc31 198 _motor4.period(period);
joe4465 0:0010a5abcc31 199
joe4465 0:0010a5abcc31 200 //Disable
joe4465 0:0010a5abcc31 201 _motor1 = MOTORS_OFF;
joe4465 0:0010a5abcc31 202 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 203 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 204 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 205 }
joe4465 0:0010a5abcc31 206
joe4465 0:0010a5abcc31 207 //Setup
joe4465 0:0010a5abcc31 208 void Setup()
joe4465 6:4c207e7b1203 209 {
joe4465 0:0010a5abcc31 210 //Setup wired serial coms
joe4465 0:0010a5abcc31 211 _wiredSerial.baud(115200);
joe4465 0:0010a5abcc31 212
joe4465 6:4c207e7b1203 213 printf("\r\n");
joe4465 6:4c207e7b1203 214 printf("*********************************************************************************\r\n");
joe4465 6:4c207e7b1203 215 printf("Starting Setup\r\n");
joe4465 6:4c207e7b1203 216 printf("*********************************************************************************\r\n");
joe4465 6:4c207e7b1203 217
joe4465 9:7b194f83e567 218 //Disable buzzer
joe4465 9:7b194f83e567 219 _buzzer = 0;
joe4465 9:7b194f83e567 220
joe4465 0:0010a5abcc31 221 //Setup wireless serial coms
joe4465 0:0010a5abcc31 222 _wirelessSerial.baud(57600);
joe4465 3:82665e39f1ea 223
joe4465 0:0010a5abcc31 224 //Read config file
joe4465 0:0010a5abcc31 225 LoadSettingsFromConfig();
joe4465 0:0010a5abcc31 226
joe4465 0:0010a5abcc31 227 //Set initial RC Ccommands
joe4465 1:045edcf091f3 228 _rcMappedCommands[0] = 0;
joe4465 1:045edcf091f3 229 _rcMappedCommands[1] = 0;
joe4465 1:045edcf091f3 230 _rcMappedCommands[2] = 0;
joe4465 1:045edcf091f3 231 _rcMappedCommands[3] = 0;
joe4465 6:4c207e7b1203 232
joe4465 6:4c207e7b1203 233 //Setup RC median filters
joe4465 9:7b194f83e567 234 _yawMedianFilter = new medianFilter(5);
joe4465 9:7b194f83e567 235 _pitchMedianFilter = new medianFilter(5);
joe4465 9:7b194f83e567 236 _rollMedianFilter = new medianFilter(5);
joe4465 9:7b194f83e567 237 _thrustMedianFilter = new medianFilter(5);
joe4465 9:7b194f83e567 238
joe4465 3:82665e39f1ea 239 //Initialise PPM
joe4465 3:82665e39f1ea 240 _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
joe4465 0:0010a5abcc31 241
joe4465 0:0010a5abcc31 242 //Initialise IMU
joe4465 0:0010a5abcc31 243 _freeIMU.init(true);
joe4465 9:7b194f83e567 244
joe4465 9:7b194f83e567 245 //Initialise MaxBotix ping sensor
joe4465 9:7b194f83e567 246 _maxBotixTimer.start();
joe4465 9:7b194f83e567 247
joe4465 9:7b194f83e567 248 //Initialise GPS
joe4465 9:7b194f83e567 249 _gps.baud(115200);
joe4465 0:0010a5abcc31 250
joe4465 0:0010a5abcc31 251 //Initialise PID
joe4465 0:0010a5abcc31 252 InitialisePID();
joe4465 0:0010a5abcc31 253
joe4465 0:0010a5abcc31 254 //Initialise PWM
joe4465 0:0010a5abcc31 255 InitialisePWM();
joe4465 6:4c207e7b1203 256
joe4465 6:4c207e7b1203 257 //Set initialised flag
joe4465 6:4c207e7b1203 258 _initialised = true;
joe4465 3:82665e39f1ea 259
joe4465 0:0010a5abcc31 260 // Start threads
joe4465 6:4c207e7b1203 261 _flightControllerThread = new Thread (FlightControllerThread);
joe4465 6:4c207e7b1203 262 _flightControllerThread->set_priority(osPriorityRealtime);
joe4465 6:4c207e7b1203 263 _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
joe4465 6:4c207e7b1203 264 _rcCommandMonitorThread->set_priority(osPriorityHigh);
joe4465 9:7b194f83e567 265 _altitudeMonitorThread = new Thread (AltitudeMonitorThread);
joe4465 9:7b194f83e567 266 _altitudeMonitorThread->set_priority(osPriorityHigh);
joe4465 6:4c207e7b1203 267 _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
joe4465 6:4c207e7b1203 268 _serialPortMonitorThread->set_priority(osPriorityLow);
joe4465 6:4c207e7b1203 269 _statusThread = new Thread(StatusThread);
joe4465 6:4c207e7b1203 270 _statusThread->set_priority(osPriorityIdle);
joe4465 9:7b194f83e567 271
joe4465 9:7b194f83e567 272 Thread::wait(1000);
joe4465 9:7b194f83e567 273
joe4465 9:7b194f83e567 274 printf("*********************************************************************************\r\n");
joe4465 9:7b194f83e567 275 printf("Finished Setup\r\n");
joe4465 9:7b194f83e567 276 printf("*********************************************************************************\r\n");
joe4465 0:0010a5abcc31 277 }
joe4465 0:0010a5abcc31 278
joe4465 0:0010a5abcc31 279 int main()
joe4465 9:7b194f83e567 280 {
joe4465 0:0010a5abcc31 281 Setup();
joe4465 0:0010a5abcc31 282
joe4465 9:7b194f83e567 283 Thread::wait(osWaitForever);
joe4465 0:0010a5abcc31 284 }