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@3:82665e39f1ea, 2014-09-18 (annotated)

Committer:
joe4465
Date:
Thu Sep 18 08:45:46 2014 +0000
Revision:
3:82665e39f1ea
Parent:
1:045edcf091f3
Child:
5:7b7db24ef6eb
First revision of quadcopter software

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