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@0:0010a5abcc31, 2014-05-09 (annotated)

Committer:
joe4465
Date:
Fri May 09 10:04:36 2014 +0000
Revision:
0:0010a5abcc31
Child:
1:045edcf091f3
Added get rate function that returns the gyroscope rate - yaw, pitch, roll

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 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 0:0010a5abcc31 104 _rcCommands[0] = value; //Yaw
joe4465 0:0010a5abcc31 105 break;
joe4465 0:0010a5abcc31 106 //Set pitch
joe4465 0:0010a5abcc31 107 case 'e':
joe4465 0:0010a5abcc31 108 _rcCommands[1] = value; //Pitch
joe4465 0:0010a5abcc31 109 break;
joe4465 0:0010a5abcc31 110 //Set roll
joe4465 0:0010a5abcc31 111 case 'f':
joe4465 0:0010a5abcc31 112 _rcCommands[2] = value; //Roll
joe4465 0:0010a5abcc31 113 break;
joe4465 0:0010a5abcc31 114 //Set thrust
joe4465 0:0010a5abcc31 115 case 'g':
joe4465 0:0010a5abcc31 116 _rcCommands[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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 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 0:0010a5abcc31 207 //WriteSettingsToConfig();
joe4465 0:0010a5abcc31 208 break;
joe4465 0:0010a5abcc31 209 //Get Settings
joe4465 0:0010a5abcc31 210 case 'z':
joe4465 0:0010a5abcc31 211 //_wiredSerial.printf("%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f\n", _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
joe4465 0:0010a5abcc31 212 break;
joe4465 0:0010a5abcc31 213 //Zero pitch and roll
joe4465 0:0010a5abcc31 214 case '1':
joe4465 0:0010a5abcc31 215 ZeroPitchRoll();
joe4465 0:0010a5abcc31 216 //WriteSettingsToConfig();
joe4465 0:0010a5abcc31 217 break;
joe4465 0:0010a5abcc31 218 default:
joe4465 0:0010a5abcc31 219 break;
joe4465 0:0010a5abcc31 220 }
joe4465 0:0010a5abcc31 221
joe4465 0:0010a5abcc31 222 return;
joe4465 0:0010a5abcc31 223 }
joe4465 0:0010a5abcc31 224
joe4465 0:0010a5abcc31 225 //Saves settings to config file
joe4465 0:0010a5abcc31 226 void WriteSettingsToConfig()
joe4465 0:0010a5abcc31 227 {
joe4465 0:0010a5abcc31 228 _wiredSerial.printf("Writing settings to config file\n\r");
joe4465 0:0010a5abcc31 229
joe4465 0:0010a5abcc31 230 //Pause timer to avoid conflicts
joe4465 0:0010a5abcc31 231 //_updateTimer->stop();
joe4465 0:0010a5abcc31 232 //_freeIMU.stop_sampling();
joe4465 0:0010a5abcc31 233
joe4465 0:0010a5abcc31 234 //Write values
joe4465 0:0010a5abcc31 235 ConvertToCharArray(_yawRatePIDControllerP);
joe4465 0:0010a5abcc31 236 if (!_configFile.setValue("_yawRatePIDControllerP", _str))
joe4465 0:0010a5abcc31 237 {
joe4465 0:0010a5abcc31 238 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 239 }
joe4465 0:0010a5abcc31 240
joe4465 0:0010a5abcc31 241 ConvertToCharArray(_yawRatePIDControllerI);
joe4465 0:0010a5abcc31 242 if (!_configFile.setValue("_yawRatePIDControllerI", _str))
joe4465 0:0010a5abcc31 243 {
joe4465 0:0010a5abcc31 244 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 245 }
joe4465 0:0010a5abcc31 246
joe4465 0:0010a5abcc31 247 ConvertToCharArray(_yawRatePIDControllerD);
joe4465 0:0010a5abcc31 248 if (!_configFile.setValue("_yawRatePIDControllerD", _str))
joe4465 0:0010a5abcc31 249 {
joe4465 0:0010a5abcc31 250 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 251 }
joe4465 0:0010a5abcc31 252
joe4465 0:0010a5abcc31 253 ConvertToCharArray(_pitchRatePIDControllerP);
joe4465 0:0010a5abcc31 254 if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
joe4465 0:0010a5abcc31 255 {
joe4465 0:0010a5abcc31 256 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 257 }
joe4465 0:0010a5abcc31 258
joe4465 0:0010a5abcc31 259 ConvertToCharArray(_pitchRatePIDControllerI);
joe4465 0:0010a5abcc31 260 if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
joe4465 0:0010a5abcc31 261 {
joe4465 0:0010a5abcc31 262 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 263 }
joe4465 0:0010a5abcc31 264
joe4465 0:0010a5abcc31 265 ConvertToCharArray(_pitchRatePIDControllerD);
joe4465 0:0010a5abcc31 266 if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
joe4465 0:0010a5abcc31 267 {
joe4465 0:0010a5abcc31 268 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 269 }
joe4465 0:0010a5abcc31 270
joe4465 0:0010a5abcc31 271 ConvertToCharArray(_rollRatePIDControllerP);
joe4465 0:0010a5abcc31 272 if (!_configFile.setValue("_rollRatePIDControllerP", _str))
joe4465 0:0010a5abcc31 273 {
joe4465 0:0010a5abcc31 274 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 275 }
joe4465 0:0010a5abcc31 276
joe4465 0:0010a5abcc31 277 ConvertToCharArray(_rollRatePIDControllerI);
joe4465 0:0010a5abcc31 278 if (!_configFile.setValue("_rollRatePIDControllerI", _str))
joe4465 0:0010a5abcc31 279 {
joe4465 0:0010a5abcc31 280 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 281 }
joe4465 0:0010a5abcc31 282
joe4465 0:0010a5abcc31 283 ConvertToCharArray(_rollRatePIDControllerD);
joe4465 0:0010a5abcc31 284 if (!_configFile.setValue("_rollRatePIDControllerD", _str))
joe4465 0:0010a5abcc31 285 {
joe4465 0:0010a5abcc31 286 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 287 }
joe4465 0:0010a5abcc31 288
joe4465 0:0010a5abcc31 289 ConvertToCharArray(_yawStabPIDControllerP);
joe4465 0:0010a5abcc31 290 if (!_configFile.setValue("_yawStabPIDControllerP", _str))
joe4465 0:0010a5abcc31 291 {
joe4465 0:0010a5abcc31 292 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 293 }
joe4465 0:0010a5abcc31 294
joe4465 0:0010a5abcc31 295 ConvertToCharArray(_yawStabPIDControllerI);
joe4465 0:0010a5abcc31 296 if (!_configFile.setValue("_yawStabPIDControllerI", _str))
joe4465 0:0010a5abcc31 297 {
joe4465 0:0010a5abcc31 298 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 299 }
joe4465 0:0010a5abcc31 300
joe4465 0:0010a5abcc31 301 ConvertToCharArray(_yawStabPIDControllerD);
joe4465 0:0010a5abcc31 302 if (!_configFile.setValue("_yawStabPIDControllerD", _str))
joe4465 0:0010a5abcc31 303 {
joe4465 0:0010a5abcc31 304 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 305 }
joe4465 0:0010a5abcc31 306
joe4465 0:0010a5abcc31 307 ConvertToCharArray(_pitchStabPIDControllerP);
joe4465 0:0010a5abcc31 308 if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
joe4465 0:0010a5abcc31 309 {
joe4465 0:0010a5abcc31 310 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 311 }
joe4465 0:0010a5abcc31 312
joe4465 0:0010a5abcc31 313 ConvertToCharArray(_pitchStabPIDControllerI);
joe4465 0:0010a5abcc31 314 if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
joe4465 0:0010a5abcc31 315 {
joe4465 0:0010a5abcc31 316 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 317 }
joe4465 0:0010a5abcc31 318
joe4465 0:0010a5abcc31 319 ConvertToCharArray(_pitchStabPIDControllerD);
joe4465 0:0010a5abcc31 320 if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
joe4465 0:0010a5abcc31 321 {
joe4465 0:0010a5abcc31 322 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 323 }
joe4465 0:0010a5abcc31 324
joe4465 0:0010a5abcc31 325 ConvertToCharArray(_rollStabPIDControllerP);
joe4465 0:0010a5abcc31 326 if (!_configFile.setValue("_rollStabPIDControllerP", _str))
joe4465 0:0010a5abcc31 327 {
joe4465 0:0010a5abcc31 328 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 329 }
joe4465 0:0010a5abcc31 330
joe4465 0:0010a5abcc31 331 ConvertToCharArray(_rollStabPIDControllerI);
joe4465 0:0010a5abcc31 332 if (!_configFile.setValue("_rollStabPIDControllerI", _str))
joe4465 0:0010a5abcc31 333 {
joe4465 0:0010a5abcc31 334 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 335 }
joe4465 0:0010a5abcc31 336
joe4465 0:0010a5abcc31 337 ConvertToCharArray(_rollStabPIDControllerD);
joe4465 0:0010a5abcc31 338 if (!_configFile.setValue("_rollStabPIDControllerD", _str))
joe4465 0:0010a5abcc31 339 {
joe4465 0:0010a5abcc31 340 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 341 }
joe4465 0:0010a5abcc31 342
joe4465 0:0010a5abcc31 343 ConvertToCharArray(_zeroValues[1]);
joe4465 0:0010a5abcc31 344 if (!_configFile.setValue("_zeroPitch", _str))
joe4465 0:0010a5abcc31 345 {
joe4465 0:0010a5abcc31 346 _wiredSerial.printf("Failed to write value for zero pitch\n\r");
joe4465 0:0010a5abcc31 347 }
joe4465 0:0010a5abcc31 348
joe4465 0:0010a5abcc31 349 ConvertToCharArray(_zeroValues[2]);
joe4465 0:0010a5abcc31 350 if (!_configFile.setValue("_zeroRoll", _str))
joe4465 0:0010a5abcc31 351 {
joe4465 0:0010a5abcc31 352 _wiredSerial.printf("Failed to write value for zero roll\n\r");
joe4465 0:0010a5abcc31 353 }
joe4465 0:0010a5abcc31 354
joe4465 0:0010a5abcc31 355 if (!_configFile.write("/local/config.cfg"))
joe4465 0:0010a5abcc31 356 {
joe4465 0:0010a5abcc31 357 _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
joe4465 0:0010a5abcc31 358 }
joe4465 0:0010a5abcc31 359 else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
joe4465 0:0010a5abcc31 360
joe4465 0:0010a5abcc31 361 //Start timer
joe4465 0:0010a5abcc31 362 //int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
joe4465 0:0010a5abcc31 363 //_updateTimer->start(updateTime);
joe4465 0:0010a5abcc31 364 //_freeIMU.start_sampling();
joe4465 0:0010a5abcc31 365 }
joe4465 0:0010a5abcc31 366
joe4465 0:0010a5abcc31 367 //Converts float to char array
joe4465 0:0010a5abcc31 368 void ConvertToCharArray(float number)
joe4465 0:0010a5abcc31 369 {
joe4465 0:0010a5abcc31 370 sprintf(_str, "%1.6f", number );
joe4465 0:0010a5abcc31 371 }
joe4465 0:0010a5abcc31 372
joe4465 0:0010a5abcc31 373 //Converts integer to char array
joe4465 0:0010a5abcc31 374 void ConvertToCharArray(int number)
joe4465 0:0010a5abcc31 375 {
joe4465 0:0010a5abcc31 376 sprintf(_str, "%d", number );
joe4465 0:0010a5abcc31 377 }
joe4465 0:0010a5abcc31 378
joe4465 0:0010a5abcc31 379 //Updates PID tunings
joe4465 0:0010a5abcc31 380 void UpdatePID()
joe4465 0:0010a5abcc31 381 {
joe4465 0:0010a5abcc31 382 float updateTime = 1.0 / UPDATE_FREQUENCY;
joe4465 0:0010a5abcc31 383
joe4465 0:0010a5abcc31 384 _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 385 _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 386 _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 387 _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 388 _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 389 _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 390 }
joe4465 0:0010a5abcc31 391
joe4465 0:0010a5abcc31 392 //Zero gyro, zero yaw and arm
joe4465 0:0010a5abcc31 393 void Arm()
joe4465 0:0010a5abcc31 394 {
joe4465 0:0010a5abcc31 395 //Zero gyro
joe4465 0:0010a5abcc31 396 _freeIMU.zeroGyro();
joe4465 0:0010a5abcc31 397
joe4465 0:0010a5abcc31 398 //Zero Yaw
joe4465 0:0010a5abcc31 399 int totalYaw = 0;
joe4465 0:0010a5abcc31 400 float ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 0:0010a5abcc31 401 for(int i = 0; i < 500; i++)
joe4465 0:0010a5abcc31 402 {
joe4465 0:0010a5abcc31 403 _freeIMU.getYawPitchRoll(ypr);
joe4465 0:0010a5abcc31 404 totalYaw += ypr[0];
joe4465 0:0010a5abcc31 405 }
joe4465 0:0010a5abcc31 406
joe4465 0:0010a5abcc31 407 _zeroValues[0] = totalYaw/500;
joe4465 0:0010a5abcc31 408
joe4465 0:0010a5abcc31 409 //Set armed to true
joe4465 0:0010a5abcc31 410 _armed = true;
joe4465 0:0010a5abcc31 411 }
joe4465 0:0010a5abcc31 412
joe4465 0:0010a5abcc31 413 //Zero pitch and roll
joe4465 0:0010a5abcc31 414 void ZeroPitchRoll()
joe4465 0:0010a5abcc31 415 {
joe4465 0:0010a5abcc31 416 //Zero pitch and roll
joe4465 0:0010a5abcc31 417 int totalPitch = 0;
joe4465 0:0010a5abcc31 418 int totalRoll = 0;
joe4465 0:0010a5abcc31 419 float ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 0:0010a5abcc31 420 for(int i = 0; i < 500; i++)
joe4465 0:0010a5abcc31 421 {
joe4465 0:0010a5abcc31 422 _freeIMU.getYawPitchRoll(ypr);
joe4465 0:0010a5abcc31 423 totalPitch += ypr[1];
joe4465 0:0010a5abcc31 424 totalRoll += ypr[2];
joe4465 0:0010a5abcc31 425 }
joe4465 0:0010a5abcc31 426
joe4465 0:0010a5abcc31 427 _zeroValues[1] = totalPitch/500;
joe4465 0:0010a5abcc31 428 _zeroValues[2] = totalRoll/500;
joe4465 0:0010a5abcc31 429
joe4465 0:0010a5abcc31 430 //WriteSettingsToConfig();
joe4465 0:0010a5abcc31 431 }