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?
User | Revision | Line number | New 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 | } |