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

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