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@5:7b7db24ef6eb, 2014-09-22 (annotated)

Committer:
joe4465
Date:
Mon Sep 22 10:16:31 2014 +0000
Revision:
5:7b7db24ef6eb
Parent:
3:82665e39f1ea
Child:
6:4c207e7b1203
Testing with Euler angles instead of YPR

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 5:7b7db24ef6eb 38 //Print to windows application
joe4465 3:82665e39f1ea 39 int batt = 0;
joe4465 3:82665e39f1ea 40 _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 41 _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 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 5:7b7db24ef6eb 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 5:7b7db24ef6eb 102 //<Command=Value>
joe4465 3:82665e39f1ea 103 void CheckWirelessSerialCommand()
joe4465 0:0010a5abcc31 104 {
joe4465 3:82665e39f1ea 105 int length = _wirelessSerialRxPos;
joe4465 3:82665e39f1ea 106 _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
joe4465 3:82665e39f1ea 107 _wirelessSerialRxPos = 0;
joe4465 0:0010a5abcc31 108
joe4465 0:0010a5abcc31 109 if (length < 1)
joe4465 0:0010a5abcc31 110 {
joe4465 0:0010a5abcc31 111 return;
joe4465 0:0010a5abcc31 112 }
joe4465 0:0010a5abcc31 113
joe4465 3:82665e39f1ea 114 char command = _wirelessSerialBuffer[0];
joe4465 0:0010a5abcc31 115 double value = 0;
joe4465 0:0010a5abcc31 116 if(length > 1)
joe4465 0:0010a5abcc31 117 {
joe4465 3:82665e39f1ea 118 value = atof((char*)&_wirelessSerialBuffer[2]);
joe4465 0:0010a5abcc31 119 }
joe4465 0:0010a5abcc31 120
joe4465 0:0010a5abcc31 121 switch (command)
joe4465 0:0010a5abcc31 122 {
joe4465 0:0010a5abcc31 123 //Spare
joe4465 0:0010a5abcc31 124 case 'a':
joe4465 0:0010a5abcc31 125 break;
joe4465 0:0010a5abcc31 126 //Arm disarm
joe4465 0:0010a5abcc31 127 case 'b':
joe4465 0:0010a5abcc31 128 if(_initialised == true && _armed == false)
joe4465 0:0010a5abcc31 129 {
joe4465 0:0010a5abcc31 130 Arm();
joe4465 0:0010a5abcc31 131 }
joe4465 0:0010a5abcc31 132 else
joe4465 0:0010a5abcc31 133 {
joe4465 3:82665e39f1ea 134 Disarm();
joe4465 0:0010a5abcc31 135 }
joe4465 0:0010a5abcc31 136 break;
joe4465 0:0010a5abcc31 137 //Set mode
joe4465 0:0010a5abcc31 138 case 'c':
joe4465 0:0010a5abcc31 139 if(_rate == true)
joe4465 0:0010a5abcc31 140 {
joe4465 0:0010a5abcc31 141 _rate = false;
joe4465 0:0010a5abcc31 142 _stab = true;
joe4465 0:0010a5abcc31 143 }
joe4465 0:0010a5abcc31 144 else
joe4465 0:0010a5abcc31 145 {
joe4465 0:0010a5abcc31 146 _rate = true;
joe4465 0:0010a5abcc31 147 _stab = false;
joe4465 0:0010a5abcc31 148 }
joe4465 0:0010a5abcc31 149 break;
joe4465 0:0010a5abcc31 150 //Set yaw
joe4465 0:0010a5abcc31 151 case 'd':
joe4465 3:82665e39f1ea 152 if(_armed == true) _rcMappedCommands[0] = value; //Yaw
joe4465 0:0010a5abcc31 153 break;
joe4465 0:0010a5abcc31 154 //Set pitch
joe4465 0:0010a5abcc31 155 case 'e':
joe4465 3:82665e39f1ea 156 if(_armed == true) _rcMappedCommands[1] = value; //Pitch
joe4465 0:0010a5abcc31 157 break;
joe4465 0:0010a5abcc31 158 //Set roll
joe4465 0:0010a5abcc31 159 case 'f':
joe4465 3:82665e39f1ea 160 if(_armed == true) _rcMappedCommands[2] = value; //Roll
joe4465 0:0010a5abcc31 161 break;
joe4465 0:0010a5abcc31 162 //Set thrust
joe4465 0:0010a5abcc31 163 case 'g':
joe4465 3:82665e39f1ea 164 if(_armed == true) _rcMappedCommands[3] = value; //Thrust
joe4465 0:0010a5abcc31 165 break;
joe4465 0:0010a5abcc31 166 //Set PID values
joe4465 0:0010a5abcc31 167 case 'h':
joe4465 0:0010a5abcc31 168 _yawRatePIDControllerP = value;
joe4465 0:0010a5abcc31 169 UpdatePID();
joe4465 0:0010a5abcc31 170 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 171 break;
joe4465 0:0010a5abcc31 172 case 'i':
joe4465 0:0010a5abcc31 173 _yawRatePIDControllerI = value;
joe4465 0:0010a5abcc31 174 UpdatePID();
joe4465 1:045edcf091f3 175 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 176 break;
joe4465 0:0010a5abcc31 177 case 'j':
joe4465 0:0010a5abcc31 178 _yawRatePIDControllerD = value;
joe4465 0:0010a5abcc31 179 UpdatePID();
joe4465 1:045edcf091f3 180 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 181 break;
joe4465 0:0010a5abcc31 182 case 'k':
joe4465 0:0010a5abcc31 183 _pitchRatePIDControllerP = value;
joe4465 0:0010a5abcc31 184 UpdatePID();
joe4465 1:045edcf091f3 185 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 186 break;
joe4465 0:0010a5abcc31 187 case 'l':
joe4465 0:0010a5abcc31 188 _pitchRatePIDControllerI = value;
joe4465 0:0010a5abcc31 189 UpdatePID();
joe4465 1:045edcf091f3 190 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 191 break;
joe4465 0:0010a5abcc31 192 case 'm':
joe4465 0:0010a5abcc31 193 _pitchRatePIDControllerD = value;
joe4465 0:0010a5abcc31 194 UpdatePID();
joe4465 1:045edcf091f3 195 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 196 break;
joe4465 0:0010a5abcc31 197 case 'n':
joe4465 0:0010a5abcc31 198 _rollRatePIDControllerP = value;
joe4465 0:0010a5abcc31 199 UpdatePID();
joe4465 1:045edcf091f3 200 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 201 break;
joe4465 0:0010a5abcc31 202 case 'o':
joe4465 0:0010a5abcc31 203 _rollRatePIDControllerI = value;
joe4465 0:0010a5abcc31 204 UpdatePID();
joe4465 1:045edcf091f3 205 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 206 break;
joe4465 0:0010a5abcc31 207 case 'p':
joe4465 0:0010a5abcc31 208 _rollRatePIDControllerD = value;
joe4465 0:0010a5abcc31 209 UpdatePID();
joe4465 1:045edcf091f3 210 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 211 break;
joe4465 0:0010a5abcc31 212 case 'q':
joe4465 0:0010a5abcc31 213 _yawStabPIDControllerP = value;
joe4465 0:0010a5abcc31 214 UpdatePID();
joe4465 1:045edcf091f3 215 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 216 break;
joe4465 0:0010a5abcc31 217 case 'r':
joe4465 0:0010a5abcc31 218 _yawStabPIDControllerI = value;
joe4465 0:0010a5abcc31 219 UpdatePID();
joe4465 1:045edcf091f3 220 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 221 break;
joe4465 0:0010a5abcc31 222 case 's':
joe4465 0:0010a5abcc31 223 _yawStabPIDControllerD = value;
joe4465 0:0010a5abcc31 224 UpdatePID();
joe4465 1:045edcf091f3 225 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 226 break;
joe4465 0:0010a5abcc31 227 case 't':
joe4465 0:0010a5abcc31 228 _pitchStabPIDControllerP = value;
joe4465 0:0010a5abcc31 229 UpdatePID();
joe4465 1:045edcf091f3 230 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 231 break;
joe4465 0:0010a5abcc31 232 case 'u':
joe4465 0:0010a5abcc31 233 _pitchStabPIDControllerI = value;
joe4465 0:0010a5abcc31 234 UpdatePID();
joe4465 1:045edcf091f3 235 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 236 break;
joe4465 0:0010a5abcc31 237 case 'v':
joe4465 0:0010a5abcc31 238 _pitchStabPIDControllerD = value;
joe4465 0:0010a5abcc31 239 UpdatePID();
joe4465 1:045edcf091f3 240 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 241 break;
joe4465 0:0010a5abcc31 242 case 'w':
joe4465 0:0010a5abcc31 243 _rollStabPIDControllerP = value;
joe4465 0:0010a5abcc31 244 UpdatePID();
joe4465 1:045edcf091f3 245 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 246 break;
joe4465 0:0010a5abcc31 247 case 'x':
joe4465 0:0010a5abcc31 248 _rollStabPIDControllerI = value;
joe4465 0:0010a5abcc31 249 UpdatePID();
joe4465 1:045edcf091f3 250 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 251 break;
joe4465 0:0010a5abcc31 252 case 'y':
joe4465 0:0010a5abcc31 253 _rollStabPIDControllerD = value;
joe4465 0:0010a5abcc31 254 UpdatePID();
joe4465 1:045edcf091f3 255 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 256 break;
joe4465 0:0010a5abcc31 257 //Zero pitch and roll
joe4465 0:0010a5abcc31 258 case '1':
joe4465 0:0010a5abcc31 259 ZeroPitchRoll();
joe4465 1:045edcf091f3 260 WriteSettingsToConfig();
joe4465 0:0010a5abcc31 261 break;
joe4465 0:0010a5abcc31 262 default:
joe4465 0:0010a5abcc31 263 break;
joe4465 0:0010a5abcc31 264 }
joe4465 0:0010a5abcc31 265
joe4465 0:0010a5abcc31 266 return;
joe4465 0:0010a5abcc31 267 }
joe4465 0:0010a5abcc31 268
joe4465 3:82665e39f1ea 269 //Checks for a valid command from the serial port and executes it
joe4465 5:7b7db24ef6eb 270 //<%.2f:%.2f:%.2f:%.2f:%.2f:%d>", latitude, longitude, GnssInfo.altitude.meters(), GnssInfo.course.deg(), GnssInfo.speed.kph(), _connectedFlag);
joe4465 3:82665e39f1ea 271 /*void CheckGPSSerialCommand()
joe4465 3:82665e39f1ea 272 {
joe4465 3:82665e39f1ea 273 int length = _gpsSerialRxPos;
joe4465 3:82665e39f1ea 274 _gpsSerialBuffer[_gpsSerialRxPos] = 0;
joe4465 3:82665e39f1ea 275 _gpsSerialRxPos = 0;
joe4465 3:82665e39f1ea 276
joe4465 3:82665e39f1ea 277 if (length < 1)
joe4465 3:82665e39f1ea 278 {
joe4465 3:82665e39f1ea 279 return;
joe4465 3:82665e39f1ea 280 }
joe4465 3:82665e39f1ea 281
joe4465 3:82665e39f1ea 282 printf("command\r\n");
joe4465 3:82665e39f1ea 283
joe4465 5:7b7db24ef6eb 284 //Convert _gpsSerialBuffer to string
joe4465 5:7b7db24ef6eb 285
joe4465 5:7b7db24ef6eb 286 //Split on :
joe4465 5:7b7db24ef6eb 287
joe4465 5:7b7db24ef6eb 288 //Check it split into 6 parts
joe4465 5:7b7db24ef6eb 289
joe4465 5:7b7db24ef6eb 290 //Convert to cirrect format and assign to vars
joe4465 5:7b7db24ef6eb 291
joe4465 3:82665e39f1ea 292 char command = _gpsSerialBuffer[0];
joe4465 3:82665e39f1ea 293 double value = 0;
joe4465 3:82665e39f1ea 294 if(length > 1)
joe4465 3:82665e39f1ea 295 {
joe4465 3:82665e39f1ea 296 value = atof((char*)&_gpsSerialBuffer[2]);
joe4465 3:82665e39f1ea 297 }
joe4465 3:82665e39f1ea 298
joe4465 3:82665e39f1ea 299 switch (command)
joe4465 3:82665e39f1ea 300 {
joe4465 3:82665e39f1ea 301 //Latitude
joe4465 3:82665e39f1ea 302 case 'a':
joe4465 3:82665e39f1ea 303 _gpsValues[0] = value;
joe4465 3:82665e39f1ea 304 _gpsConnected = true;
joe4465 3:82665e39f1ea 305 break;
joe4465 3:82665e39f1ea 306 //Longitude
joe4465 3:82665e39f1ea 307 case 'b':
joe4465 3:82665e39f1ea 308 _gpsValues[1] = value;
joe4465 3:82665e39f1ea 309 _gpsConnected = true;
joe4465 3:82665e39f1ea 310 break;
joe4465 3:82665e39f1ea 311 //Altitude
joe4465 3:82665e39f1ea 312 case 'c':
joe4465 3:82665e39f1ea 313 _gpsValues[2] = value;
joe4465 3:82665e39f1ea 314 _gpsConnected = true;
joe4465 3:82665e39f1ea 315 break;
joe4465 3:82665e39f1ea 316 //Course
joe4465 3:82665e39f1ea 317 case 'd':
joe4465 3:82665e39f1ea 318 _gpsValues[3] = value;
joe4465 3:82665e39f1ea 319 _gpsConnected = true;
joe4465 3:82665e39f1ea 320 break;
joe4465 3:82665e39f1ea 321 //Speed
joe4465 3:82665e39f1ea 322 case 'e':
joe4465 3:82665e39f1ea 323 _gpsValues[4] = value;
joe4465 3:82665e39f1ea 324 _gpsConnected = true;
joe4465 3:82665e39f1ea 325 break;
joe4465 3:82665e39f1ea 326 //Not Connected
joe4465 3:82665e39f1ea 327 case 'f':
joe4465 3:82665e39f1ea 328 _gpsConnected = false;
joe4465 3:82665e39f1ea 329 break;
joe4465 3:82665e39f1ea 330
joe4465 3:82665e39f1ea 331 default:
joe4465 3:82665e39f1ea 332 break;
joe4465 3:82665e39f1ea 333 }
joe4465 3:82665e39f1ea 334
joe4465 3:82665e39f1ea 335 return;
joe4465 3:82665e39f1ea 336 }*/
joe4465 3:82665e39f1ea 337
joe4465 0:0010a5abcc31 338 //Saves settings to config file
joe4465 0:0010a5abcc31 339 void WriteSettingsToConfig()
joe4465 0:0010a5abcc31 340 {
joe4465 0:0010a5abcc31 341 _wiredSerial.printf("Writing settings to config file\n\r");
joe4465 0:0010a5abcc31 342
joe4465 3:82665e39f1ea 343 if(_armed == false) //Not flying
joe4465 0:0010a5abcc31 344 {
joe4465 1:045edcf091f3 345 _freeIMU.sample(false);
joe4465 1:045edcf091f3 346
joe4465 1:045edcf091f3 347 //Write values
joe4465 1:045edcf091f3 348 ConvertToCharArray(_yawRatePIDControllerP);
joe4465 1:045edcf091f3 349 if (!_configFile.setValue("_yawRatePIDControllerP", _str))
joe4465 1:045edcf091f3 350 {
joe4465 1:045edcf091f3 351 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 352 }
joe4465 1:045edcf091f3 353
joe4465 1:045edcf091f3 354 ConvertToCharArray(_yawRatePIDControllerI);
joe4465 1:045edcf091f3 355 if (!_configFile.setValue("_yawRatePIDControllerI", _str))
joe4465 1:045edcf091f3 356 {
joe4465 1:045edcf091f3 357 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 358 }
joe4465 1:045edcf091f3 359
joe4465 1:045edcf091f3 360 ConvertToCharArray(_yawRatePIDControllerD);
joe4465 1:045edcf091f3 361 if (!_configFile.setValue("_yawRatePIDControllerD", _str))
joe4465 1:045edcf091f3 362 {
joe4465 1:045edcf091f3 363 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 364 }
joe4465 1:045edcf091f3 365
joe4465 1:045edcf091f3 366 ConvertToCharArray(_pitchRatePIDControllerP);
joe4465 1:045edcf091f3 367 if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
joe4465 1:045edcf091f3 368 {
joe4465 1:045edcf091f3 369 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 370 }
joe4465 1:045edcf091f3 371
joe4465 1:045edcf091f3 372 ConvertToCharArray(_pitchRatePIDControllerI);
joe4465 1:045edcf091f3 373 if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
joe4465 1:045edcf091f3 374 {
joe4465 1:045edcf091f3 375 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 376 }
joe4465 1:045edcf091f3 377
joe4465 1:045edcf091f3 378 ConvertToCharArray(_pitchRatePIDControllerD);
joe4465 1:045edcf091f3 379 if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
joe4465 1:045edcf091f3 380 {
joe4465 1:045edcf091f3 381 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 382 }
joe4465 1:045edcf091f3 383
joe4465 1:045edcf091f3 384 ConvertToCharArray(_rollRatePIDControllerP);
joe4465 1:045edcf091f3 385 if (!_configFile.setValue("_rollRatePIDControllerP", _str))
joe4465 1:045edcf091f3 386 {
joe4465 1:045edcf091f3 387 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
joe4465 1:045edcf091f3 388 }
joe4465 1:045edcf091f3 389
joe4465 1:045edcf091f3 390 ConvertToCharArray(_rollRatePIDControllerI);
joe4465 1:045edcf091f3 391 if (!_configFile.setValue("_rollRatePIDControllerI", _str))
joe4465 1:045edcf091f3 392 {
joe4465 1:045edcf091f3 393 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
joe4465 1:045edcf091f3 394 }
joe4465 1:045edcf091f3 395
joe4465 1:045edcf091f3 396 ConvertToCharArray(_rollRatePIDControllerD);
joe4465 1:045edcf091f3 397 if (!_configFile.setValue("_rollRatePIDControllerD", _str))
joe4465 1:045edcf091f3 398 {
joe4465 1:045edcf091f3 399 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
joe4465 1:045edcf091f3 400 }
joe4465 0:0010a5abcc31 401
joe4465 1:045edcf091f3 402 ConvertToCharArray(_yawStabPIDControllerP);
joe4465 1:045edcf091f3 403 if (!_configFile.setValue("_yawStabPIDControllerP", _str))
joe4465 1:045edcf091f3 404 {
joe4465 1:045edcf091f3 405 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 406 }
joe4465 1:045edcf091f3 407
joe4465 1:045edcf091f3 408 ConvertToCharArray(_yawStabPIDControllerI);
joe4465 1:045edcf091f3 409 if (!_configFile.setValue("_yawStabPIDControllerI", _str))
joe4465 1:045edcf091f3 410 {
joe4465 1:045edcf091f3 411 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 412 }
joe4465 1:045edcf091f3 413
joe4465 1:045edcf091f3 414 ConvertToCharArray(_yawStabPIDControllerD);
joe4465 1:045edcf091f3 415 if (!_configFile.setValue("_yawStabPIDControllerD", _str))
joe4465 1:045edcf091f3 416 {
joe4465 1:045edcf091f3 417 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 418 }
joe4465 1:045edcf091f3 419
joe4465 1:045edcf091f3 420 ConvertToCharArray(_pitchStabPIDControllerP);
joe4465 1:045edcf091f3 421 if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
joe4465 1:045edcf091f3 422 {
joe4465 1:045edcf091f3 423 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 424 }
joe4465 1:045edcf091f3 425
joe4465 1:045edcf091f3 426 ConvertToCharArray(_pitchStabPIDControllerI);
joe4465 1:045edcf091f3 427 if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
joe4465 1:045edcf091f3 428 {
joe4465 1:045edcf091f3 429 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 430 }
joe4465 1:045edcf091f3 431
joe4465 1:045edcf091f3 432 ConvertToCharArray(_pitchStabPIDControllerD);
joe4465 1:045edcf091f3 433 if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
joe4465 1:045edcf091f3 434 {
joe4465 1:045edcf091f3 435 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 436 }
joe4465 1:045edcf091f3 437
joe4465 1:045edcf091f3 438 ConvertToCharArray(_rollStabPIDControllerP);
joe4465 1:045edcf091f3 439 if (!_configFile.setValue("_rollStabPIDControllerP", _str))
joe4465 1:045edcf091f3 440 {
joe4465 1:045edcf091f3 441 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
joe4465 1:045edcf091f3 442 }
joe4465 1:045edcf091f3 443
joe4465 1:045edcf091f3 444 ConvertToCharArray(_rollStabPIDControllerI);
joe4465 1:045edcf091f3 445 if (!_configFile.setValue("_rollStabPIDControllerI", _str))
joe4465 1:045edcf091f3 446 {
joe4465 1:045edcf091f3 447 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
joe4465 1:045edcf091f3 448 }
joe4465 1:045edcf091f3 449
joe4465 1:045edcf091f3 450 ConvertToCharArray(_rollStabPIDControllerD);
joe4465 1:045edcf091f3 451 if (!_configFile.setValue("_rollStabPIDControllerD", _str))
joe4465 1:045edcf091f3 452 {
joe4465 1:045edcf091f3 453 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
joe4465 1:045edcf091f3 454 }
joe4465 0:0010a5abcc31 455
joe4465 1:045edcf091f3 456 ConvertToCharArray(_zeroValues[1]);
joe4465 1:045edcf091f3 457 if (!_configFile.setValue("_zeroPitch", _str))
joe4465 1:045edcf091f3 458 {
joe4465 1:045edcf091f3 459 _wiredSerial.printf("Failed to write value for zero pitch\n\r");
joe4465 1:045edcf091f3 460 }
joe4465 1:045edcf091f3 461
joe4465 1:045edcf091f3 462 ConvertToCharArray(_zeroValues[2]);
joe4465 1:045edcf091f3 463 if (!_configFile.setValue("_zeroRoll", _str))
joe4465 1:045edcf091f3 464 {
joe4465 1:045edcf091f3 465 _wiredSerial.printf("Failed to write value for zero roll\n\r");
joe4465 1:045edcf091f3 466 }
joe4465 1:045edcf091f3 467
joe4465 1:045edcf091f3 468 if (!_configFile.write("/local/config.cfg"))
joe4465 1:045edcf091f3 469 {
joe4465 1:045edcf091f3 470 _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
joe4465 1:045edcf091f3 471 }
joe4465 1:045edcf091f3 472 else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
joe4465 1:045edcf091f3 473
joe4465 1:045edcf091f3 474 _freeIMU.sample(true);
joe4465 0:0010a5abcc31 475 }
joe4465 1:045edcf091f3 476 else
joe4465 0:0010a5abcc31 477 {
joe4465 1:045edcf091f3 478 _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
joe4465 0:0010a5abcc31 479 }
joe4465 0:0010a5abcc31 480 }
joe4465 0:0010a5abcc31 481
joe4465 0:0010a5abcc31 482 //Converts float to char array
joe4465 0:0010a5abcc31 483 void ConvertToCharArray(float number)
joe4465 0:0010a5abcc31 484 {
joe4465 1:045edcf091f3 485 sprintf(_str, "%1.8f", number );
joe4465 0:0010a5abcc31 486 }
joe4465 0:0010a5abcc31 487
joe4465 0:0010a5abcc31 488 //Converts integer to char array
joe4465 0:0010a5abcc31 489 void ConvertToCharArray(int number)
joe4465 0:0010a5abcc31 490 {
joe4465 0:0010a5abcc31 491 sprintf(_str, "%d", number );
joe4465 0:0010a5abcc31 492 }
joe4465 0:0010a5abcc31 493
joe4465 0:0010a5abcc31 494 //Updates PID tunings
joe4465 0:0010a5abcc31 495 void UpdatePID()
joe4465 0:0010a5abcc31 496 {
joe4465 1:045edcf091f3 497 _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD);
joe4465 1:045edcf091f3 498 _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD);
joe4465 1:045edcf091f3 499 _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD);
joe4465 1:045edcf091f3 500 _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD);
joe4465 1:045edcf091f3 501 _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD);
joe4465 1:045edcf091f3 502 _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 0:0010a5abcc31 503 }