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