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@3:82665e39f1ea, 2014-09-18 (annotated)

Committer:
joe4465
Date:
Thu Sep 18 08:45:46 2014 +0000
Revision:
3:82665e39f1ea
Parent:
1:045edcf091f3
Child:
6:4c207e7b1203
First revision of quadcopter software

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