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@9:7b194f83e567, 2015-02-22 (annotated)

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 #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 9:7b194f83e567 6 void CheckSerialCommand();
joe4465 0:0010a5abcc31 7 void UpdatePID();
joe4465 9:7b194f83e567 8 void getGPS();
joe4465 0:0010a5abcc31 9
joe4465 0:0010a5abcc31 10 //Variables
joe4465 3:82665e39f1ea 11 char _wirelessSerialBuffer[255];
joe4465 3:82665e39f1ea 12 int _wirelessSerialRxPos = 0;
joe4465 0:0010a5abcc31 13
joe4465 0:0010a5abcc31 14 // A thread to monitor the serial ports
joe4465 0:0010a5abcc31 15 void SerialPortMonitorThread(void const *args)
joe4465 6:4c207e7b1203 16 {
joe4465 6:4c207e7b1203 17 printf("Serial port monitor thread started\r\n");
joe4465 3:82665e39f1ea 18
joe4465 6:4c207e7b1203 19 while(true)
joe4465 6:4c207e7b1203 20 {
joe4465 6:4c207e7b1203 21 //Check comms mode and print correct data back to PC application
joe4465 6:4c207e7b1203 22 switch(_commsMode)
joe4465 0:0010a5abcc31 23 {
joe4465 6:4c207e7b1203 24 //Motor power
joe4465 6:4c207e7b1203 25 case 0:
joe4465 6:4c207e7b1203 26 _wirelessSerial.printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>",
joe4465 6:4c207e7b1203 27 _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3]);
joe4465 6:4c207e7b1203 28 break;
joe4465 6:4c207e7b1203 29
joe4465 6:4c207e7b1203 30 //PID outputs
joe4465 6:4c207e7b1203 31 case 1:
joe4465 6:4c207e7b1203 32 _wirelessSerial.printf("<SYPID=%1.2f:SPPID=%1.2f:SRPID=%1.2f:RYPID=%1.2f:RPPID=%1.2f:RRPID=%1.2f>",
joe4465 6:4c207e7b1203 33 _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2]);
joe4465 6:4c207e7b1203 34 break;
joe4465 6:4c207e7b1203 35
joe4465 6:4c207e7b1203 36 //IMU outputs
joe4465 6:4c207e7b1203 37 case 2:
joe4465 6:4c207e7b1203 38 _wirelessSerial.printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>",
joe4465 6:4c207e7b1203 39 _ypr[0], _ypr[1], _ypr[2], _gyroRate[0], _gyroRate[1], _gyroRate[2]);
joe4465 3:82665e39f1ea 40 break;
joe4465 0:0010a5abcc31 41
joe4465 6:4c207e7b1203 42 //Status
joe4465 6:4c207e7b1203 43 case 3:
joe4465 6:4c207e7b1203 44 _wirelessSerial.printf("<Batt=%d:Armed=%d:Init=%d:Rate=%d:Stab=%d:Lev=%d>",
joe4465 6:4c207e7b1203 45 _batt, _armed, _initialised, _rate, _stab, _levelOffset);
joe4465 6:4c207e7b1203 46 break;
joe4465 6:4c207e7b1203 47
joe4465 6:4c207e7b1203 48 //Mapped RC commands
joe4465 6:4c207e7b1203 49 case 4:
joe4465 9:7b194f83e567 50 _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f:RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
joe4465 9:7b194f83e567 51 _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]);
joe4465 6:4c207e7b1203 52 break;
joe4465 6:4c207e7b1203 53
joe4465 6:4c207e7b1203 54 //PID Tuning
joe4465 6:4c207e7b1203 55 case 5:
joe4465 9:7b194f83e567 56 _wirelessSerial.printf("<RYPIDP=%1.6f:RYPIDI=%1.6f:RYPIDD=%1.6f:RPPIDP=%1.6f:RPPIDI=%1.6f:RPPIDD=%1.6f:RRPIDP=%1.6f:RRPIDI=%1.6f:RRPIDD=%1.6f:SYPIDP=%1.6f:SYPIDI=%1.6f:SYPIDD=%1.6f:SPPIDP=%1.6f:SPPIDI=%1.6f:SPPIDD=%1.6f:SRPIDP=%1.6f:SRPIDI=%1.6f:SRPIDD=%1.6f>",
joe4465 6:4c207e7b1203 57 _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 6:4c207e7b1203 58 break;
joe4465 6:4c207e7b1203 59
joe4465 6:4c207e7b1203 60 //GPS
joe4465 6:4c207e7b1203 61 case 6:
joe4465 6:4c207e7b1203 62 _wirelessSerial.printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GAng=%1.2f:GSpd=%1.2f:GInit=%d>",
joe4465 6:4c207e7b1203 63 _gpsValues[0], _gpsValues[1], _gpsValues[2], _gpsValues[3], _gpsValues[4], _gpsConnected);
joe4465 6:4c207e7b1203 64 break;
joe4465 6:4c207e7b1203 65
joe4465 6:4c207e7b1203 66 //Zero mode
joe4465 6:4c207e7b1203 67 case 7:
joe4465 6:4c207e7b1203 68 _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
joe4465 6:4c207e7b1203 69 _zeroValues[0], _zeroValues[1], _zeroValues[2]);
joe4465 9:7b194f83e567 70 break;
joe4465 6:4c207e7b1203 71
joe4465 9:7b194f83e567 72 //Rate tuning
joe4465 6:4c207e7b1203 73 case 8:
joe4465 9:7b194f83e567 74 //Yaw set point, Yaw actual, Yaw PID output
joe4465 9:7b194f83e567 75 //Pitch set point, Pitch actual, Pitch PID output
joe4465 9:7b194f83e567 76 //Roll set point, Roll actual, Roll PID output
joe4465 9:7b194f83e567 77 _wirelessSerial.printf("<MRCY=%1.2f:RY=%1.2f:RYPID=%1.2f:MRCP=%1.2f:RP=%1.2f:RPPID=%1.2f:MRCR=%1.2f:RR=%1.2f:RRPID=%1.2f>",
joe4465 9:7b194f83e567 78 _rcMappedCommands[0], _gyroRate[0], _ratePIDControllerOutputs[0], _rcMappedCommands[1], _gyroRate[1], _ratePIDControllerOutputs[1], _rcMappedCommands[2], _gyroRate[2], _ratePIDControllerOutputs[2]);
joe4465 3:82665e39f1ea 79 break;
joe4465 0:0010a5abcc31 80
joe4465 9:7b194f83e567 81 //Stab tuning
joe4465 9:7b194f83e567 82 case 9:
joe4465 9:7b194f83e567 83 //Yaw set point, Yaw actual, Yaw PID output
joe4465 9:7b194f83e567 84 //Pitch set point, Pitch actual, Pitch PID output
joe4465 9:7b194f83e567 85 //Roll set point, Roll actual, Roll PID output
joe4465 9:7b194f83e567 86 _wirelessSerial.printf("<MRCY=%1.2f:SY=%1.2f:SYPID=%1.2f:MRCP=%1.2f:SP=%1.2f:SPPID=%1.2f:MRCR=%1.2f:SR=%1.2f:SRPID=%1.2f>",
joe4465 9:7b194f83e567 87 _rcMappedCommands[0], _ypr[0], _stabPIDControllerOutputs[0], _rcMappedCommands[1], _ypr[1], _stabPIDControllerOutputs[1], _rcMappedCommands[2], _ypr[2], _stabPIDControllerOutputs[2]);
joe4465 9:7b194f83e567 88 break;
joe4465 9:7b194f83e567 89
joe4465 9:7b194f83e567 90 //Altitude
joe4465 9:7b194f83e567 91 case 10:
joe4465 9:7b194f83e567 92 _wirelessSerial.printf("<GAlt=%1.2f:PAlt=%1.2f:BAlt=%1.2f>",
joe4465 9:7b194f83e567 93 _gpsValues[2], _maxBotixPingAltitude, _barometerAltitude);
joe4465 9:7b194f83e567 94
joe4465 3:82665e39f1ea 95 default:
joe4465 6:4c207e7b1203 96 break;
joe4465 9:7b194f83e567 97 }
joe4465 3:82665e39f1ea 98
joe4465 6:4c207e7b1203 99 //Check for wireless serial command
joe4465 9:7b194f83e567 100 while (_wirelessSerial.readable() > 0)
joe4465 3:82665e39f1ea 101 {
joe4465 6:4c207e7b1203 102 int c = _wirelessSerial.getc();
joe4465 9:7b194f83e567 103
joe4465 6:4c207e7b1203 104 switch (c)
joe4465 6:4c207e7b1203 105 {
joe4465 6:4c207e7b1203 106 case 60: //
joe4465 6:4c207e7b1203 107 _wirelessSerialRxPos = 0;
joe4465 6:4c207e7b1203 108 break;
joe4465 3:82665e39f1ea 109
joe4465 6:4c207e7b1203 110 case 10: // LF
joe4465 6:4c207e7b1203 111 case 13: // CR
joe4465 6:4c207e7b1203 112 case 62: // >
joe4465 9:7b194f83e567 113 CheckSerialCommand();
joe4465 6:4c207e7b1203 114 break;
joe4465 6:4c207e7b1203 115
joe4465 6:4c207e7b1203 116 default:
joe4465 6:4c207e7b1203 117 _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
joe4465 6:4c207e7b1203 118 if (_wirelessSerialRxPos > 200)
joe4465 6:4c207e7b1203 119 {
joe4465 6:4c207e7b1203 120 _wirelessSerialRxPos = 0;
joe4465 6:4c207e7b1203 121 }
joe4465 6:4c207e7b1203 122 break;
joe4465 6:4c207e7b1203 123 }
joe4465 9:7b194f83e567 124 }
joe4465 9:7b194f83e567 125
joe4465 6:4c207e7b1203 126 //Check for GPS serial command
joe4465 9:7b194f83e567 127 while(_gps.readable() > 0)
joe4465 6:4c207e7b1203 128 {
joe4465 9:7b194f83e567 129 int c = _gps.getc();
joe4465 9:7b194f83e567 130 if(_tinyGPS.encode(c))
joe4465 6:4c207e7b1203 131 {
joe4465 9:7b194f83e567 132 getGPS();
joe4465 6:4c207e7b1203 133 }
joe4465 9:7b194f83e567 134 }
joe4465 9:7b194f83e567 135
joe4465 6:4c207e7b1203 136 Thread::wait(100);
joe4465 6:4c207e7b1203 137 }
joe4465 0:0010a5abcc31 138 }
joe4465 0:0010a5abcc31 139
joe4465 0:0010a5abcc31 140 //Checks for a valid command from the serial port and executes it
joe4465 5:7b7db24ef6eb 141 //<Command=Value>
joe4465 9:7b194f83e567 142 void CheckSerialCommand()
joe4465 0:0010a5abcc31 143 {
joe4465 3:82665e39f1ea 144 int length = _wirelessSerialRxPos;
joe4465 3:82665e39f1ea 145 _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
joe4465 3:82665e39f1ea 146 _wirelessSerialRxPos = 0;
joe4465 0:0010a5abcc31 147
joe4465 0:0010a5abcc31 148 if (length < 1)
joe4465 0:0010a5abcc31 149 {
joe4465 0:0010a5abcc31 150 return;
joe4465 0:0010a5abcc31 151 }
joe4465 0:0010a5abcc31 152
joe4465 3:82665e39f1ea 153 char command = _wirelessSerialBuffer[0];
joe4465 0:0010a5abcc31 154 double value = 0;
joe4465 0:0010a5abcc31 155 if(length > 1)
joe4465 0:0010a5abcc31 156 {
joe4465 3:82665e39f1ea 157 value = atof((char*)&_wirelessSerialBuffer[2]);
joe4465 0:0010a5abcc31 158 }
joe4465 0:0010a5abcc31 159
joe4465 0:0010a5abcc31 160 switch (command)
joe4465 0:0010a5abcc31 161 {
joe4465 6:4c207e7b1203 162 //Start level offset mode to teach quad level
joe4465 0:0010a5abcc31 163 case 'a':
joe4465 6:4c207e7b1203 164 _levelOffset = true;
joe4465 0:0010a5abcc31 165 break;
joe4465 6:4c207e7b1203 166
joe4465 0:0010a5abcc31 167 //Arm disarm
joe4465 0:0010a5abcc31 168 case 'b':
joe4465 0:0010a5abcc31 169 if(_initialised == true && _armed == false)
joe4465 0:0010a5abcc31 170 {
joe4465 0:0010a5abcc31 171 Arm();
joe4465 0:0010a5abcc31 172 }
joe4465 6:4c207e7b1203 173 else if(_armed == true)
joe4465 0:0010a5abcc31 174 {
joe4465 3:82665e39f1ea 175 Disarm();
joe4465 0:0010a5abcc31 176 }
joe4465 0:0010a5abcc31 177 break;
joe4465 6:4c207e7b1203 178
joe4465 0:0010a5abcc31 179 //Set mode
joe4465 0:0010a5abcc31 180 case 'c':
joe4465 0:0010a5abcc31 181 if(_rate == true)
joe4465 0:0010a5abcc31 182 {
joe4465 0:0010a5abcc31 183 _rate = false;
joe4465 0:0010a5abcc31 184 _stab = true;
joe4465 0:0010a5abcc31 185 }
joe4465 0:0010a5abcc31 186 else
joe4465 0:0010a5abcc31 187 {
joe4465 0:0010a5abcc31 188 _rate = true;
joe4465 0:0010a5abcc31 189 _stab = false;
joe4465 0:0010a5abcc31 190 }
joe4465 0:0010a5abcc31 191 break;
joe4465 6:4c207e7b1203 192
joe4465 0:0010a5abcc31 193 //Set yaw
joe4465 0:0010a5abcc31 194 case 'd':
joe4465 3:82665e39f1ea 195 if(_armed == true) _rcMappedCommands[0] = value; //Yaw
joe4465 6:4c207e7b1203 196 else _rcMappedCommands[0] = 0;
joe4465 0:0010a5abcc31 197 break;
joe4465 6:4c207e7b1203 198
joe4465 0:0010a5abcc31 199 //Set pitch
joe4465 0:0010a5abcc31 200 case 'e':
joe4465 3:82665e39f1ea 201 if(_armed == true) _rcMappedCommands[1] = value; //Pitch
joe4465 6:4c207e7b1203 202 else _rcMappedCommands[1] = 0;
joe4465 0:0010a5abcc31 203 break;
joe4465 6:4c207e7b1203 204
joe4465 0:0010a5abcc31 205 //Set roll
joe4465 0:0010a5abcc31 206 case 'f':
joe4465 3:82665e39f1ea 207 if(_armed == true) _rcMappedCommands[2] = value; //Roll
joe4465 6:4c207e7b1203 208 else _rcMappedCommands[2] = 0;
joe4465 0:0010a5abcc31 209 break;
joe4465 6:4c207e7b1203 210
joe4465 0:0010a5abcc31 211 //Set thrust
joe4465 0:0010a5abcc31 212 case 'g':
joe4465 3:82665e39f1ea 213 if(_armed == true) _rcMappedCommands[3] = value; //Thrust
joe4465 6:4c207e7b1203 214 else _rcMappedCommands[3] = 0;
joe4465 0:0010a5abcc31 215 break;
joe4465 6:4c207e7b1203 216
joe4465 0:0010a5abcc31 217 //Set PID values
joe4465 0:0010a5abcc31 218 case 'h':
joe4465 0:0010a5abcc31 219 _yawRatePIDControllerP = value;
joe4465 0:0010a5abcc31 220 UpdatePID();
joe4465 0:0010a5abcc31 221 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 222 break;
joe4465 6:4c207e7b1203 223
joe4465 0:0010a5abcc31 224 case 'i':
joe4465 0:0010a5abcc31 225 _yawRatePIDControllerI = value;
joe4465 0:0010a5abcc31 226 UpdatePID();
joe4465 1:045edcf091f3 227 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 228 break;
joe4465 6:4c207e7b1203 229
joe4465 0:0010a5abcc31 230 case 'j':
joe4465 0:0010a5abcc31 231 _yawRatePIDControllerD = value;
joe4465 0:0010a5abcc31 232 UpdatePID();
joe4465 1:045edcf091f3 233 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 234 break;
joe4465 6:4c207e7b1203 235
joe4465 0:0010a5abcc31 236 case 'k':
joe4465 0:0010a5abcc31 237 _pitchRatePIDControllerP = value;
joe4465 0:0010a5abcc31 238 UpdatePID();
joe4465 1:045edcf091f3 239 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 240 break;
joe4465 6:4c207e7b1203 241
joe4465 0:0010a5abcc31 242 case 'l':
joe4465 0:0010a5abcc31 243 _pitchRatePIDControllerI = value;
joe4465 0:0010a5abcc31 244 UpdatePID();
joe4465 1:045edcf091f3 245 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 246 break;
joe4465 6:4c207e7b1203 247
joe4465 0:0010a5abcc31 248 case 'm':
joe4465 0:0010a5abcc31 249 _pitchRatePIDControllerD = value;
joe4465 0:0010a5abcc31 250 UpdatePID();
joe4465 1:045edcf091f3 251 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 252 break;
joe4465 6:4c207e7b1203 253
joe4465 0:0010a5abcc31 254 case 'n':
joe4465 0:0010a5abcc31 255 _rollRatePIDControllerP = value;
joe4465 0:0010a5abcc31 256 UpdatePID();
joe4465 1:045edcf091f3 257 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 258 break;
joe4465 6:4c207e7b1203 259
joe4465 0:0010a5abcc31 260 case 'o':
joe4465 0:0010a5abcc31 261 _rollRatePIDControllerI = value;
joe4465 0:0010a5abcc31 262 UpdatePID();
joe4465 1:045edcf091f3 263 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 264 break;
joe4465 6:4c207e7b1203 265
joe4465 0:0010a5abcc31 266 case 'p':
joe4465 0:0010a5abcc31 267 _rollRatePIDControllerD = value;
joe4465 0:0010a5abcc31 268 UpdatePID();
joe4465 1:045edcf091f3 269 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 270 break;
joe4465 6:4c207e7b1203 271
joe4465 0:0010a5abcc31 272 case 'q':
joe4465 0:0010a5abcc31 273 _yawStabPIDControllerP = value;
joe4465 0:0010a5abcc31 274 UpdatePID();
joe4465 1:045edcf091f3 275 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 276 break;
joe4465 6:4c207e7b1203 277
joe4465 0:0010a5abcc31 278 case 'r':
joe4465 0:0010a5abcc31 279 _yawStabPIDControllerI = value;
joe4465 0:0010a5abcc31 280 UpdatePID();
joe4465 1:045edcf091f3 281 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 282 break;
joe4465 6:4c207e7b1203 283
joe4465 0:0010a5abcc31 284 case 's':
joe4465 0:0010a5abcc31 285 _yawStabPIDControllerD = value;
joe4465 0:0010a5abcc31 286 UpdatePID();
joe4465 1:045edcf091f3 287 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 288 break;
joe4465 6:4c207e7b1203 289
joe4465 0:0010a5abcc31 290 case 't':
joe4465 0:0010a5abcc31 291 _pitchStabPIDControllerP = value;
joe4465 0:0010a5abcc31 292 UpdatePID();
joe4465 1:045edcf091f3 293 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 294 break;
joe4465 6:4c207e7b1203 295
joe4465 0:0010a5abcc31 296 case 'u':
joe4465 0:0010a5abcc31 297 _pitchStabPIDControllerI = value;
joe4465 0:0010a5abcc31 298 UpdatePID();
joe4465 1:045edcf091f3 299 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 300 break;
joe4465 6:4c207e7b1203 301
joe4465 0:0010a5abcc31 302 case 'v':
joe4465 0:0010a5abcc31 303 _pitchStabPIDControllerD = value;
joe4465 0:0010a5abcc31 304 UpdatePID();
joe4465 1:045edcf091f3 305 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 306 break;
joe4465 6:4c207e7b1203 307
joe4465 0:0010a5abcc31 308 case 'w':
joe4465 0:0010a5abcc31 309 _rollStabPIDControllerP = value;
joe4465 0:0010a5abcc31 310 UpdatePID();
joe4465 1:045edcf091f3 311 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 312 break;
joe4465 6:4c207e7b1203 313
joe4465 0:0010a5abcc31 314 case 'x':
joe4465 0:0010a5abcc31 315 _rollStabPIDControllerI = value;
joe4465 0:0010a5abcc31 316 UpdatePID();
joe4465 1:045edcf091f3 317 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 318 break;
joe4465 6:4c207e7b1203 319
joe4465 0:0010a5abcc31 320 case 'y':
joe4465 0:0010a5abcc31 321 _rollStabPIDControllerD = value;
joe4465 0:0010a5abcc31 322 UpdatePID();
joe4465 1:045edcf091f3 323 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 324 break;
joe4465 6:4c207e7b1203 325
joe4465 6:4c207e7b1203 326 case 'z':
joe4465 6:4c207e7b1203 327 _commsMode = value;
joe4465 6:4c207e7b1203 328 break;
joe4465 6:4c207e7b1203 329
joe4465 0:0010a5abcc31 330 case '1':
joe4465 7:bc5822aa8878 331 _levelOffset = false;
joe4465 6:4c207e7b1203 332 _zeroValues[0] = 0;
joe4465 6:4c207e7b1203 333 _zeroValues[1] = 0;
joe4465 6:4c207e7b1203 334 _zeroValues[2] = 0;
joe4465 1:045edcf091f3 335 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 336 break;
joe4465 6:4c207e7b1203 337
joe4465 0:0010a5abcc31 338 default:
joe4465 0:0010a5abcc31 339 break;
joe4465 0:0010a5abcc31 340 }
joe4465 0:0010a5abcc31 341
joe4465 0:0010a5abcc31 342 return;
joe4465 0:0010a5abcc31 343 }
joe4465 0:0010a5abcc31 344
joe4465 9:7b194f83e567 345 void getGPS()
joe4465 9:7b194f83e567 346 {
joe4465 9:7b194f83e567 347 unsigned long fix_age;
joe4465 9:7b194f83e567 348 _tinyGPS.f_get_position(&_gpsValues[0], &_gpsValues[1], &fix_age);
joe4465 9:7b194f83e567 349
joe4465 9:7b194f83e567 350 _gpsValues[2] = _tinyGPS.f_altitude();
joe4465 9:7b194f83e567 351 _gpsValues[3] = _tinyGPS.f_course();
joe4465 9:7b194f83e567 352 _gpsValues[4] = _tinyGPS.f_speed_kmph();
joe4465 3:82665e39f1ea 353
joe4465 9:7b194f83e567 354 if (fix_age == TinyGPS::GPS_INVALID_AGE)
joe4465 9:7b194f83e567 355 _gpsConnected = false;
joe4465 9:7b194f83e567 356 else if (fix_age > 5000)
joe4465 9:7b194f83e567 357 _gpsConnected = false;
joe4465 9:7b194f83e567 358 else
joe4465 9:7b194f83e567 359 _gpsConnected = true;
joe4465 9:7b194f83e567 360 }
joe4465 3:82665e39f1ea 361
joe4465 0:0010a5abcc31 362 //Updates PID tunings
joe4465 0:0010a5abcc31 363 void UpdatePID()
joe4465 0:0010a5abcc31 364 {
joe4465 1:045edcf091f3 365 _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD);
joe4465 1:045edcf091f3 366 _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD);
joe4465 1:045edcf091f3 367 _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD);
joe4465 1:045edcf091f3 368 _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD);
joe4465 1:045edcf091f3 369 _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD);
joe4465 1:045edcf091f3 370 _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 0:0010a5abcc31 371 }