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@7:bc5822aa8878, 2015-02-10 (annotated)

Committer:
joe4465
Date:
Tue Feb 10 20:55:44 2015 +0000
Revision:
7:bc5822aa8878
Parent:
6:4c207e7b1203
Child:
9:7b194f83e567
Updated software to match new PCB

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