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