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

serialPortMonitor.h@1:045edcf091f3, 2014-05-16 (annotated)

Committer:
joe4465
Date:
Fri May 16 14:18:05 2014 +0000
Revision:
1:045edcf091f3
Parent:
0:0010a5abcc31
Child:
3:82665e39f1ea
first commit

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 void CheckCommand();
joe4465 0:0010a5abcc31 7 void ZeroPitchRoll();
joe4465 0:0010a5abcc31 8 void WriteSettingsToConfig();
joe4465 0:0010a5abcc31 9 void ConvertToCharArray(float number);
joe4465 0:0010a5abcc31 10 void ConvertToCharArray(int number);
joe4465 0:0010a5abcc31 11 void UpdatePID();
joe4465 0:0010a5abcc31 12 void Arm();
joe4465 0:0010a5abcc31 13
joe4465 0:0010a5abcc31 14 //Variables
joe4465 0:0010a5abcc31 15 char* _str = new char[1024];
joe4465 0:0010a5abcc31 16 char _serialBuffer[255];
joe4465 0:0010a5abcc31 17 int _serialRxPos = 0;
joe4465 0:0010a5abcc31 18
joe4465 0:0010a5abcc31 19 // A thread to monitor the serial ports
joe4465 0:0010a5abcc31 20 void SerialPortMonitorThread(void const *args)
joe4465 0:0010a5abcc31 21 {
joe4465 0:0010a5abcc31 22 while(true)
joe4465 0:0010a5abcc31 23 {
joe4465 0:0010a5abcc31 24 //Check for serial command
joe4465 0:0010a5abcc31 25 if (_wirelessSerial.readable() > 0)
joe4465 0:0010a5abcc31 26 {
joe4465 0:0010a5abcc31 27 int c = _wirelessSerial.getc();
joe4465 0:0010a5abcc31 28
joe4465 0:0010a5abcc31 29 switch (c)
joe4465 0:0010a5abcc31 30 {
joe4465 0:0010a5abcc31 31 case 60: //
joe4465 0:0010a5abcc31 32 _serialRxPos = 0;
joe4465 0:0010a5abcc31 33 break;
joe4465 0:0010a5abcc31 34
joe4465 0:0010a5abcc31 35 case 10: // LF
joe4465 0:0010a5abcc31 36 case 13: // CR
joe4465 0:0010a5abcc31 37 case 62: // >
joe4465 0:0010a5abcc31 38 CheckCommand();
joe4465 0:0010a5abcc31 39 break;
joe4465 0:0010a5abcc31 40
joe4465 0:0010a5abcc31 41 default:
joe4465 0:0010a5abcc31 42 _serialBuffer[_serialRxPos++] = c;
joe4465 0:0010a5abcc31 43 if (_serialRxPos > 200)
joe4465 0:0010a5abcc31 44 {
joe4465 0:0010a5abcc31 45 _serialRxPos = 0;
joe4465 0:0010a5abcc31 46 }
joe4465 0:0010a5abcc31 47 break;
joe4465 0:0010a5abcc31 48 }
joe4465 0:0010a5abcc31 49 }
joe4465 0:0010a5abcc31 50 Thread::wait(100);
joe4465 0:0010a5abcc31 51 }
joe4465 0:0010a5abcc31 52 }
joe4465 0:0010a5abcc31 53
joe4465 0:0010a5abcc31 54 //Checks for a valid command from the serial port and executes it
joe4465 0:0010a5abcc31 55 void CheckCommand()
joe4465 0:0010a5abcc31 56 {
joe4465 0:0010a5abcc31 57 int length = _serialRxPos;
joe4465 0:0010a5abcc31 58 _serialBuffer[_serialRxPos] = 0;
joe4465 0:0010a5abcc31 59 _serialRxPos = 0;
joe4465 0:0010a5abcc31 60
joe4465 0:0010a5abcc31 61 if (length < 1)
joe4465 0:0010a5abcc31 62 {
joe4465 0:0010a5abcc31 63 return;
joe4465 0:0010a5abcc31 64 }
joe4465 0:0010a5abcc31 65
joe4465 0:0010a5abcc31 66 char command = _serialBuffer[0];
joe4465 0:0010a5abcc31 67 double value = 0;
joe4465 0:0010a5abcc31 68 if(length > 1)
joe4465 0:0010a5abcc31 69 {
joe4465 0:0010a5abcc31 70 value = atof((char*)&_serialBuffer[2]);
joe4465 0:0010a5abcc31 71 }
joe4465 0:0010a5abcc31 72
joe4465 0:0010a5abcc31 73 switch (command)
joe4465 0:0010a5abcc31 74 {
joe4465 0:0010a5abcc31 75 //Spare
joe4465 0:0010a5abcc31 76 case 'a':
joe4465 0:0010a5abcc31 77 break;
joe4465 0:0010a5abcc31 78 //Arm disarm
joe4465 0:0010a5abcc31 79 case 'b':
joe4465 0:0010a5abcc31 80 if(_initialised == true && _armed == false)
joe4465 0:0010a5abcc31 81 {
joe4465 0:0010a5abcc31 82 Arm();
joe4465 0:0010a5abcc31 83 }
joe4465 0:0010a5abcc31 84 else
joe4465 0:0010a5abcc31 85 {
joe4465 0:0010a5abcc31 86 _armed = false;
joe4465 0:0010a5abcc31 87 }
joe4465 0:0010a5abcc31 88 break;
joe4465 0:0010a5abcc31 89 //Set mode
joe4465 0:0010a5abcc31 90 case 'c':
joe4465 0:0010a5abcc31 91 if(_rate == true)
joe4465 0:0010a5abcc31 92 {
joe4465 0:0010a5abcc31 93 _rate = false;
joe4465 0:0010a5abcc31 94 _stab = true;
joe4465 0:0010a5abcc31 95 }
joe4465 0:0010a5abcc31 96 else
joe4465 0:0010a5abcc31 97 {
joe4465 0:0010a5abcc31 98 _rate = true;
joe4465 0:0010a5abcc31 99 _stab = false;
joe4465 0:0010a5abcc31 100 }
joe4465 0:0010a5abcc31 101 break;
joe4465 0:0010a5abcc31 102 //Set yaw
joe4465 0:0010a5abcc31 103 case 'd':
joe4465 1:045edcf091f3 104 //_rcMappedCommands[0] = value; //Yaw
joe4465 0:0010a5abcc31 105 break;
joe4465 0:0010a5abcc31 106 //Set pitch
joe4465 0:0010a5abcc31 107 case 'e':
joe4465 1:045edcf091f3 108 //_rcMappedCommands[1] = value; //Pitch
joe4465 0:0010a5abcc31 109 break;
joe4465 0:0010a5abcc31 110 //Set roll
joe4465 0:0010a5abcc31 111 case 'f':
joe4465 1:045edcf091f3 112 //_rcMappedCommands[2] = value; //Roll
joe4465 0:0010a5abcc31 113 break;
joe4465 0:0010a5abcc31 114 //Set thrust
joe4465 0:0010a5abcc31 115 case 'g':
joe4465 1:045edcf091f3 116 //_rcMappedCommands[3] = value; //Thrust
joe4465 0:0010a5abcc31 117 break;
joe4465 0:0010a5abcc31 118 //Set PID values
joe4465 0:0010a5abcc31 119 case 'h':
joe4465 0:0010a5abcc31 120 _yawRatePIDControllerP = value;
joe4465 0:0010a5abcc31 121 UpdatePID();
joe4465 0:0010a5abcc31 122 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 123 break;
joe4465 0:0010a5abcc31 124 case 'i':
joe4465 0:0010a5abcc31 125 _yawRatePIDControllerI = value;
joe4465 0:0010a5abcc31 126 UpdatePID();
joe4465 1:045edcf091f3 127 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 128 break;
joe4465 0:0010a5abcc31 129 case 'j':
joe4465 0:0010a5abcc31 130 _yawRatePIDControllerD = value;
joe4465 0:0010a5abcc31 131 UpdatePID();
joe4465 1:045edcf091f3 132 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 133 break;
joe4465 0:0010a5abcc31 134 case 'k':
joe4465 0:0010a5abcc31 135 _pitchRatePIDControllerP = value;
joe4465 0:0010a5abcc31 136 UpdatePID();
joe4465 1:045edcf091f3 137 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 138 break;
joe4465 0:0010a5abcc31 139 case 'l':
joe4465 0:0010a5abcc31 140 _pitchRatePIDControllerI = value;
joe4465 0:0010a5abcc31 141 UpdatePID();
joe4465 1:045edcf091f3 142 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 143 break;
joe4465 0:0010a5abcc31 144 case 'm':
joe4465 0:0010a5abcc31 145 _pitchRatePIDControllerD = value;
joe4465 0:0010a5abcc31 146 UpdatePID();
joe4465 1:045edcf091f3 147 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 148 break;
joe4465 0:0010a5abcc31 149 case 'n':
joe4465 0:0010a5abcc31 150 _rollRatePIDControllerP = value;
joe4465 0:0010a5abcc31 151 UpdatePID();
joe4465 1:045edcf091f3 152 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 153 break;
joe4465 0:0010a5abcc31 154 case 'o':
joe4465 0:0010a5abcc31 155 _rollRatePIDControllerI = value;
joe4465 0:0010a5abcc31 156 UpdatePID();
joe4465 1:045edcf091f3 157 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 158 break;
joe4465 0:0010a5abcc31 159 case 'p':
joe4465 0:0010a5abcc31 160 _rollRatePIDControllerD = value;
joe4465 0:0010a5abcc31 161 UpdatePID();
joe4465 1:045edcf091f3 162 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 163 break;
joe4465 0:0010a5abcc31 164 case 'q':
joe4465 0:0010a5abcc31 165 _yawStabPIDControllerP = value;
joe4465 0:0010a5abcc31 166 UpdatePID();
joe4465 1:045edcf091f3 167 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 168 break;
joe4465 0:0010a5abcc31 169 case 'r':
joe4465 0:0010a5abcc31 170 _yawStabPIDControllerI = value;
joe4465 0:0010a5abcc31 171 UpdatePID();
joe4465 1:045edcf091f3 172 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 173 break;
joe4465 0:0010a5abcc31 174 case 's':
joe4465 0:0010a5abcc31 175 _yawStabPIDControllerD = value;
joe4465 0:0010a5abcc31 176 UpdatePID();
joe4465 1:045edcf091f3 177 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 178 break;
joe4465 0:0010a5abcc31 179 case 't':
joe4465 0:0010a5abcc31 180 _pitchStabPIDControllerP = value;
joe4465 0:0010a5abcc31 181 UpdatePID();
joe4465 1:045edcf091f3 182 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 183 break;
joe4465 0:0010a5abcc31 184 case 'u':
joe4465 0:0010a5abcc31 185 _pitchStabPIDControllerI = value;
joe4465 0:0010a5abcc31 186 UpdatePID();
joe4465 1:045edcf091f3 187 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 188 break;
joe4465 0:0010a5abcc31 189 case 'v':
joe4465 0:0010a5abcc31 190 _pitchStabPIDControllerD = value;
joe4465 0:0010a5abcc31 191 UpdatePID();
joe4465 1:045edcf091f3 192 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 193 break;
joe4465 0:0010a5abcc31 194 case 'w':
joe4465 0:0010a5abcc31 195 _rollStabPIDControllerP = value;
joe4465 0:0010a5abcc31 196 UpdatePID();
joe4465 1:045edcf091f3 197 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 198 break;
joe4465 0:0010a5abcc31 199 case 'x':
joe4465 0:0010a5abcc31 200 _rollStabPIDControllerI = value;
joe4465 0:0010a5abcc31 201 UpdatePID();
joe4465 1:045edcf091f3 202 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 203 break;
joe4465 0:0010a5abcc31 204 case 'y':
joe4465 0:0010a5abcc31 205 _rollStabPIDControllerD = value;
joe4465 0:0010a5abcc31 206 UpdatePID();
joe4465 1:045edcf091f3 207 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 208 break;
joe4465 0:0010a5abcc31 209 //Zero pitch and roll
joe4465 0:0010a5abcc31 210 case '1':
joe4465 0:0010a5abcc31 211 ZeroPitchRoll();
joe4465 1:045edcf091f3 212 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 213 break;
joe4465 0:0010a5abcc31 214 default:
joe4465 0:0010a5abcc31 215 break;
joe4465 0:0010a5abcc31 216 }
joe4465 0:0010a5abcc31 217
joe4465 0:0010a5abcc31 218 return;
joe4465 0:0010a5abcc31 219 }
joe4465 0:0010a5abcc31 220
joe4465 0:0010a5abcc31 221 //Saves settings to config file
joe4465 0:0010a5abcc31 222 void WriteSettingsToConfig()
joe4465 0:0010a5abcc31 223 {
joe4465 0:0010a5abcc31 224 _wiredSerial.printf("Writing settings to config file\n\r");
joe4465 0:0010a5abcc31 225
joe4465 1:045edcf091f3 226 if(_rcMappedCommands[3] < 0) //Not flying
joe4465 0:0010a5abcc31 227 {
joe4465 1:045edcf091f3 228 _freeIMU.sample(false);
joe4465 1:045edcf091f3 229
joe4465 1:045edcf091f3 230 //Write values
joe4465 1:045edcf091f3 231 ConvertToCharArray(_yawRatePIDControllerP);
joe4465 1:045edcf091f3 232 if (!_configFile.setValue("_yawRatePIDControllerP", _str))
joe4465 1:045edcf091f3 233 {
joe4465 1:045edcf091f3 234 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 235 }
joe4465 1:045edcf091f3 236
joe4465 1:045edcf091f3 237 ConvertToCharArray(_yawRatePIDControllerI);
joe4465 1:045edcf091f3 238 if (!_configFile.setValue("_yawRatePIDControllerI", _str))
joe4465 1:045edcf091f3 239 {
joe4465 1:045edcf091f3 240 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 241 }
joe4465 1:045edcf091f3 242
joe4465 1:045edcf091f3 243 ConvertToCharArray(_yawRatePIDControllerD);
joe4465 1:045edcf091f3 244 if (!_configFile.setValue("_yawRatePIDControllerD", _str))
joe4465 1:045edcf091f3 245 {
joe4465 1:045edcf091f3 246 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 247 }
joe4465 1:045edcf091f3 248
joe4465 1:045edcf091f3 249 ConvertToCharArray(_pitchRatePIDControllerP);
joe4465 1:045edcf091f3 250 if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
joe4465 1:045edcf091f3 251 {
joe4465 1:045edcf091f3 252 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 253 }
joe4465 1:045edcf091f3 254
joe4465 1:045edcf091f3 255 ConvertToCharArray(_pitchRatePIDControllerI);
joe4465 1:045edcf091f3 256 if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
joe4465 1:045edcf091f3 257 {
joe4465 1:045edcf091f3 258 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 259 }
joe4465 1:045edcf091f3 260
joe4465 1:045edcf091f3 261 ConvertToCharArray(_pitchRatePIDControllerD);
joe4465 1:045edcf091f3 262 if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
joe4465 1:045edcf091f3 263 {
joe4465 1:045edcf091f3 264 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 265 }
joe4465 1:045edcf091f3 266
joe4465 1:045edcf091f3 267 ConvertToCharArray(_rollRatePIDControllerP);
joe4465 1:045edcf091f3 268 if (!_configFile.setValue("_rollRatePIDControllerP", _str))
joe4465 1:045edcf091f3 269 {
joe4465 1:045edcf091f3 270 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 271 }
joe4465 1:045edcf091f3 272
joe4465 1:045edcf091f3 273 ConvertToCharArray(_rollRatePIDControllerI);
joe4465 1:045edcf091f3 274 if (!_configFile.setValue("_rollRatePIDControllerI", _str))
joe4465 1:045edcf091f3 275 {
joe4465 1:045edcf091f3 276 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 277 }
joe4465 1:045edcf091f3 278
joe4465 1:045edcf091f3 279 ConvertToCharArray(_rollRatePIDControllerD);
joe4465 1:045edcf091f3 280 if (!_configFile.setValue("_rollRatePIDControllerD", _str))
joe4465 1:045edcf091f3 281 {
joe4465 1:045edcf091f3 282 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 283 }
joe4465 0:0010a5abcc31 284
joe4465 1:045edcf091f3 285 ConvertToCharArray(_yawStabPIDControllerP);
joe4465 1:045edcf091f3 286 if (!_configFile.setValue("_yawStabPIDControllerP", _str))
joe4465 1:045edcf091f3 287 {
joe4465 1:045edcf091f3 288 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 289 }
joe4465 1:045edcf091f3 290
joe4465 1:045edcf091f3 291 ConvertToCharArray(_yawStabPIDControllerI);
joe4465 1:045edcf091f3 292 if (!_configFile.setValue("_yawStabPIDControllerI", _str))
joe4465 1:045edcf091f3 293 {
joe4465 1:045edcf091f3 294 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 295 }
joe4465 1:045edcf091f3 296
joe4465 1:045edcf091f3 297 ConvertToCharArray(_yawStabPIDControllerD);
joe4465 1:045edcf091f3 298 if (!_configFile.setValue("_yawStabPIDControllerD", _str))
joe4465 1:045edcf091f3 299 {
joe4465 1:045edcf091f3 300 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 301 }
joe4465 1:045edcf091f3 302
joe4465 1:045edcf091f3 303 ConvertToCharArray(_pitchStabPIDControllerP);
joe4465 1:045edcf091f3 304 if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
joe4465 1:045edcf091f3 305 {
joe4465 1:045edcf091f3 306 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 307 }
joe4465 1:045edcf091f3 308
joe4465 1:045edcf091f3 309 ConvertToCharArray(_pitchStabPIDControllerI);
joe4465 1:045edcf091f3 310 if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
joe4465 1:045edcf091f3 311 {
joe4465 1:045edcf091f3 312 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 313 }
joe4465 1:045edcf091f3 314
joe4465 1:045edcf091f3 315 ConvertToCharArray(_pitchStabPIDControllerD);
joe4465 1:045edcf091f3 316 if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
joe4465 1:045edcf091f3 317 {
joe4465 1:045edcf091f3 318 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 319 }
joe4465 1:045edcf091f3 320
joe4465 1:045edcf091f3 321 ConvertToCharArray(_rollStabPIDControllerP);
joe4465 1:045edcf091f3 322 if (!_configFile.setValue("_rollStabPIDControllerP", _str))
joe4465 1:045edcf091f3 323 {
joe4465 1:045edcf091f3 324 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 325 }
joe4465 1:045edcf091f3 326
joe4465 1:045edcf091f3 327 ConvertToCharArray(_rollStabPIDControllerI);
joe4465 1:045edcf091f3 328 if (!_configFile.setValue("_rollStabPIDControllerI", _str))
joe4465 1:045edcf091f3 329 {
joe4465 1:045edcf091f3 330 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 331 }
joe4465 1:045edcf091f3 332
joe4465 1:045edcf091f3 333 ConvertToCharArray(_rollStabPIDControllerD);
joe4465 1:045edcf091f3 334 if (!_configFile.setValue("_rollStabPIDControllerD", _str))
joe4465 1:045edcf091f3 335 {
joe4465 1:045edcf091f3 336 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 337 }
joe4465 0:0010a5abcc31 338
joe4465 1:045edcf091f3 339 ConvertToCharArray(_zeroValues[1]);
joe4465 1:045edcf091f3 340 if (!_configFile.setValue("_zeroPitch", _str))
joe4465 1:045edcf091f3 341 {
joe4465 1:045edcf091f3 342 _wiredSerial.printf("Failed to write value for zero pitch\n\r");
joe4465 1:045edcf091f3 343 }
joe4465 1:045edcf091f3 344
joe4465 1:045edcf091f3 345 ConvertToCharArray(_zeroValues[2]);
joe4465 1:045edcf091f3 346 if (!_configFile.setValue("_zeroRoll", _str))
joe4465 1:045edcf091f3 347 {
joe4465 1:045edcf091f3 348 _wiredSerial.printf("Failed to write value for zero roll\n\r");
joe4465 1:045edcf091f3 349 }
joe4465 1:045edcf091f3 350
joe4465 1:045edcf091f3 351 if (!_configFile.write("/local/config.cfg"))
joe4465 1:045edcf091f3 352 {
joe4465 1:045edcf091f3 353 _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
joe4465 1:045edcf091f3 354 }
joe4465 1:045edcf091f3 355 else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
joe4465 1:045edcf091f3 356
joe4465 1:045edcf091f3 357 _freeIMU.sample(true);
joe4465 0:0010a5abcc31 358 }
joe4465 1:045edcf091f3 359 else
joe4465 0:0010a5abcc31 360 {
joe4465 1:045edcf091f3 361 _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
joe4465 0:0010a5abcc31 362 }
joe4465 0:0010a5abcc31 363 }
joe4465 0:0010a5abcc31 364
joe4465 0:0010a5abcc31 365 //Converts float to char array
joe4465 0:0010a5abcc31 366 void ConvertToCharArray(float number)
joe4465 0:0010a5abcc31 367 {
joe4465 1:045edcf091f3 368 sprintf(_str, "%1.8f", number );
joe4465 0:0010a5abcc31 369 }
joe4465 0:0010a5abcc31 370
joe4465 0:0010a5abcc31 371 //Converts integer to char array
joe4465 0:0010a5abcc31 372 void ConvertToCharArray(int number)
joe4465 0:0010a5abcc31 373 {
joe4465 0:0010a5abcc31 374 sprintf(_str, "%d", number );
joe4465 0:0010a5abcc31 375 }
joe4465 0:0010a5abcc31 376
joe4465 0:0010a5abcc31 377 //Updates PID tunings
joe4465 0:0010a5abcc31 378 void UpdatePID()
joe4465 0:0010a5abcc31 379 {
joe4465 1:045edcf091f3 380 _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD);
joe4465 1:045edcf091f3 381 _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD);
joe4465 1:045edcf091f3 382 _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD);
joe4465 1:045edcf091f3 383 _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD);
joe4465 1:045edcf091f3 384 _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD);
joe4465 1:045edcf091f3 385 _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 0:0010a5abcc31 386 }
joe4465 0:0010a5abcc31 387
joe4465 0:0010a5abcc31 388 //Zero gyro, zero yaw and arm
joe4465 0:0010a5abcc31 389 void Arm()
joe4465 0:0010a5abcc31 390 {
joe4465 0:0010a5abcc31 391 //Zero gyro
joe4465 0:0010a5abcc31 392 _freeIMU.zeroGyro();
joe4465 0:0010a5abcc31 393
joe4465 0:0010a5abcc31 394 //Set armed to true
joe4465 0:0010a5abcc31 395 _armed = true;
joe4465 0:0010a5abcc31 396 }
joe4465 0:0010a5abcc31 397
joe4465 0:0010a5abcc31 398 //Zero pitch and roll
joe4465 0:0010a5abcc31 399 void ZeroPitchRoll()
joe4465 0:0010a5abcc31 400 {
joe4465 0:0010a5abcc31 401 //Zero pitch and roll
joe4465 1:045edcf091f3 402 float totalPitch = 0;
joe4465 1:045edcf091f3 403 float totalRoll = 0;
joe4465 0:0010a5abcc31 404 float ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 0:0010a5abcc31 405 for(int i = 0; i < 500; i++)
joe4465 0:0010a5abcc31 406 {
joe4465 0:0010a5abcc31 407 _freeIMU.getYawPitchRoll(ypr);
joe4465 0:0010a5abcc31 408 totalPitch += ypr[1];
joe4465 0:0010a5abcc31 409 totalRoll += ypr[2];
joe4465 0:0010a5abcc31 410 }
joe4465 0:0010a5abcc31 411
joe4465 0:0010a5abcc31 412 _zeroValues[1] = totalPitch/500;
joe4465 0:0010a5abcc31 413 _zeroValues[2] = totalRoll/500;
joe4465 1:045edcf091f3 414 printf("Pitch %f\r\n", _zeroValues[1]);
joe4465 1:045edcf091f3 415 printf("Roll %f\r\n", _zeroValues[2]);
joe4465 0:0010a5abcc31 416 }