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?
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 | 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 | } |