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
hardware.h@6:4c207e7b1203, 2015-01-22 (annotated)
- Committer:
- joe4465
- Date:
- Thu Jan 22 18:03:22 2015 +0000
- Revision:
- 6:4c207e7b1203
- Parent:
- 3:82665e39f1ea
- Child:
- 7:bc5822aa8878
Updated Communications to PC to handle issue with long messages
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 "FreeIMU.h" |
joe4465 | 0:0010a5abcc31 | 4 | #include "PID.h" |
joe4465 | 0:0010a5abcc31 | 5 | #include "ConfigFile.h" |
joe4465 | 3:82665e39f1ea | 6 | #include "PPM.h" |
joe4465 | 6:4c207e7b1203 | 7 | #include "filter.h" |
joe4465 | 0:0010a5abcc31 | 8 | #include <sstream> |
joe4465 | 0:0010a5abcc31 | 9 | |
joe4465 | 0:0010a5abcc31 | 10 | #ifndef HARDWARE_H |
joe4465 | 0:0010a5abcc31 | 11 | #define HARDWARE_H |
joe4465 | 0:0010a5abcc31 | 12 | |
joe4465 | 6:4c207e7b1203 | 13 | //Global constants |
joe4465 | 3:82665e39f1ea | 14 | #define IMU_YAW_ANGLE_MAX 180 |
joe4465 | 3:82665e39f1ea | 15 | #define IMU_YAW_ANGLE_MIN -180 |
joe4465 | 3:82665e39f1ea | 16 | #define IMU_ROLL_ANGLE_MAX 90 |
joe4465 | 3:82665e39f1ea | 17 | #define IMU_ROLL_ANGLE_MIN -90 |
joe4465 | 3:82665e39f1ea | 18 | #define IMU_PITCH_ANGLE_MAX 90 |
joe4465 | 3:82665e39f1ea | 19 | #define IMU_PITCH_ANGLE_MIN -90 |
joe4465 | 3:82665e39f1ea | 20 | #define IMU_YAW_RATE_MAX 360 |
joe4465 | 3:82665e39f1ea | 21 | #define IMU_YAW_RATE_MIN -360 |
joe4465 | 3:82665e39f1ea | 22 | #define IMU_ROLL_RATE_MAX 360 |
joe4465 | 3:82665e39f1ea | 23 | #define IMU_ROLL_RATE_MIN -360 |
joe4465 | 3:82665e39f1ea | 24 | #define IMU_PITCH_RATE_MAX 360 |
joe4465 | 3:82665e39f1ea | 25 | #define IMU_PITCH_RATE_MIN -360 |
joe4465 | 3:82665e39f1ea | 26 | |
joe4465 | 3:82665e39f1ea | 27 | #define RC_CHANNELS 8 |
joe4465 | 3:82665e39f1ea | 28 | #define RC_THROTTLE_CHANNEL 3 |
joe4465 | 3:82665e39f1ea | 29 | #define RC_IN_MAX 1900 |
joe4465 | 3:82665e39f1ea | 30 | #define RC_IN_MIN 1000 |
joe4465 | 3:82665e39f1ea | 31 | #define RC_OUT_MAX 1 |
joe4465 | 3:82665e39f1ea | 32 | #define RC_OUT_MIN 0 |
joe4465 | 6:4c207e7b1203 | 33 | #define RC_YAW_RATE_MAX 90 |
joe4465 | 6:4c207e7b1203 | 34 | #define RC_YAW_RATE_MIN -90 |
joe4465 | 6:4c207e7b1203 | 35 | #define RC_ROLL_RATE_MAX 90 |
joe4465 | 6:4c207e7b1203 | 36 | #define RC_ROLL_RATE_MIN -90 |
joe4465 | 6:4c207e7b1203 | 37 | #define RC_PITCH_RATE_MAX 90 |
joe4465 | 6:4c207e7b1203 | 38 | #define RC_PITCH_RATE_MIN -90 |
joe4465 | 6:4c207e7b1203 | 39 | #define RC_ROLL_ANGLE_MAX 15 |
joe4465 | 6:4c207e7b1203 | 40 | #define RC_ROLL_ANGLE_MIN -15 |
joe4465 | 6:4c207e7b1203 | 41 | #define RC_PITCH_ANGLE_MAX 15 |
joe4465 | 6:4c207e7b1203 | 42 | #define RC_PITCH_ANGLE_MIN -15 |
joe4465 | 3:82665e39f1ea | 43 | #define RC_THRUST_MAX 1 |
joe4465 | 3:82665e39f1ea | 44 | #define RC_THRUST_MIN 0 |
joe4465 | 3:82665e39f1ea | 45 | |
joe4465 | 3:82665e39f1ea | 46 | #define MOTORS_OFF 0 |
joe4465 | 3:82665e39f1ea | 47 | #define MOTORS_ARMED 1000 |
joe4465 | 3:82665e39f1ea | 48 | #define MOTORS_MIN 1060 |
joe4465 | 3:82665e39f1ea | 49 | #define MOTORS_MAX 1860 |
joe4465 | 3:82665e39f1ea | 50 | |
joe4465 | 3:82665e39f1ea | 51 | #define RATE_PID_CONTROLLER_OUTPUT_MAX 100 |
joe4465 | 3:82665e39f1ea | 52 | #define RATE_PID_CONTROLLER_OUTPUT_MIN -100 |
joe4465 | 3:82665e39f1ea | 53 | |
joe4465 | 3:82665e39f1ea | 54 | #define FLIGHT_CONTROLLER_FREQUENCY 500 |
joe4465 | 3:82665e39f1ea | 55 | |
joe4465 | 6:4c207e7b1203 | 56 | //Global Functions |
joe4465 | 6:4c207e7b1203 | 57 | //void ZeroPitchRoll(); |
joe4465 | 3:82665e39f1ea | 58 | void Arm(); |
joe4465 | 3:82665e39f1ea | 59 | void Disarm(); |
joe4465 | 6:4c207e7b1203 | 60 | void WriteSettingsToConfig(); |
joe4465 | 6:4c207e7b1203 | 61 | void ConvertToCharArray(float number); |
joe4465 | 6:4c207e7b1203 | 62 | void ConvertToCharArray(int number); |
joe4465 | 3:82665e39f1ea | 63 | float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax); |
joe4465 | 0:0010a5abcc31 | 64 | |
joe4465 | 6:4c207e7b1203 | 65 | //Global Variables |
joe4465 | 3:82665e39f1ea | 66 | float _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD; |
joe4465 | 3:82665e39f1ea | 67 | float _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD; |
joe4465 | 3:82665e39f1ea | 68 | float _zeroValues[3] = {0,0,0}; //Yaw, pitch, roll |
joe4465 | 6:4c207e7b1203 | 69 | float _oldZeroValues[3] = {0,0,0}; //Yaw, pitch, roll |
joe4465 | 6:4c207e7b1203 | 70 | float _rcCommands[8] = {0,0,0,0,0,0,0,0}; |
joe4465 | 3:82665e39f1ea | 71 | float _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust |
joe4465 | 6:4c207e7b1203 | 72 | float _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed |
joe4465 | 3:82665e39f1ea | 73 | bool _armed = false; |
joe4465 | 3:82665e39f1ea | 74 | bool _rate = false; |
joe4465 | 3:82665e39f1ea | 75 | bool _stab = true; |
joe4465 | 6:4c207e7b1203 | 76 | bool _initialised = false; |
joe4465 | 6:4c207e7b1203 | 77 | bool _gpsConnected = false; |
joe4465 | 3:82665e39f1ea | 78 | float _motorPower [4] = {0,0,0,0}; |
joe4465 | 6:4c207e7b1203 | 79 | float _gyroRate[3] = {0,0,0}; // Yaw, Pitch, Roll |
joe4465 | 3:82665e39f1ea | 80 | float _ypr[3] = {0,0,0}; // Yaw, pitch, roll |
joe4465 | 3:82665e39f1ea | 81 | float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll |
joe4465 | 3:82665e39f1ea | 82 | float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll |
joe4465 | 6:4c207e7b1203 | 83 | bool _levelOffset = false; |
joe4465 | 6:4c207e7b1203 | 84 | int _commsMode = 0; |
joe4465 | 6:4c207e7b1203 | 85 | int _batt = 0; |
joe4465 | 3:82665e39f1ea | 86 | |
joe4465 | 3:82665e39f1ea | 87 | //PID controllers |
joe4465 | 3:82665e39f1ea | 88 | PID *_yawRatePIDController; |
joe4465 | 3:82665e39f1ea | 89 | PID *_pitchRatePIDController; |
joe4465 | 3:82665e39f1ea | 90 | PID *_rollRatePIDController; |
joe4465 | 3:82665e39f1ea | 91 | PID *_yawStabPIDController; |
joe4465 | 3:82665e39f1ea | 92 | PID *_pitchStabPIDController; |
joe4465 | 3:82665e39f1ea | 93 | PID *_rollStabPIDController; |
joe4465 | 0:0010a5abcc31 | 94 | |
joe4465 | 3:82665e39f1ea | 95 | //Threads |
joe4465 | 3:82665e39f1ea | 96 | Thread *_statusThread; |
joe4465 | 3:82665e39f1ea | 97 | Thread *_serialPortMonitorThread; |
joe4465 | 3:82665e39f1ea | 98 | Thread *_flightControllerThread; |
joe4465 | 3:82665e39f1ea | 99 | Thread *_rcCommandMonitorThread; |
joe4465 | 0:0010a5abcc31 | 100 | |
joe4465 | 0:0010a5abcc31 | 101 | //Config file |
joe4465 | 3:82665e39f1ea | 102 | LocalFileSystem local("local"); |
joe4465 | 3:82665e39f1ea | 103 | ConfigFile _configFile; |
joe4465 | 6:4c207e7b1203 | 104 | char* _str = new char[1024]; |
joe4465 | 6:4c207e7b1203 | 105 | |
joe4465 | 6:4c207e7b1203 | 106 | //RC filters |
joe4465 | 6:4c207e7b1203 | 107 | medianFilter *_yawMedianFilter; |
joe4465 | 6:4c207e7b1203 | 108 | medianFilter *_pitchMedianFilter; |
joe4465 | 6:4c207e7b1203 | 109 | medianFilter *_rollMedianFilter; |
joe4465 | 6:4c207e7b1203 | 110 | medianFilter *_thrustMedianFilter; |
joe4465 | 6:4c207e7b1203 | 111 | medianFilter *_channel7MedianFilter; |
joe4465 | 6:4c207e7b1203 | 112 | medianFilter *_channel8MedianFilter; |
joe4465 | 0:0010a5abcc31 | 113 | |
joe4465 | 0:0010a5abcc31 | 114 | //HARDWARE//////////////////////////////////////////////////////////////////////////////////// |
joe4465 | 0:0010a5abcc31 | 115 | // M1 M2 |
joe4465 | 0:0010a5abcc31 | 116 | // \ / |
joe4465 | 0:0010a5abcc31 | 117 | // \/ |
joe4465 | 0:0010a5abcc31 | 118 | // /\ |
joe4465 | 0:0010a5abcc31 | 119 | // / \ |
joe4465 | 0:0010a5abcc31 | 120 | // M3 M4 |
joe4465 | 0:0010a5abcc31 | 121 | |
joe4465 | 0:0010a5abcc31 | 122 | //Motors |
joe4465 | 3:82665e39f1ea | 123 | PwmOut _motor1(p22); |
joe4465 | 3:82665e39f1ea | 124 | PwmOut _motor2(p23); |
joe4465 | 3:82665e39f1ea | 125 | PwmOut _motor3(p24); |
joe4465 | 3:82665e39f1ea | 126 | PwmOut _motor4(p25); |
joe4465 | 0:0010a5abcc31 | 127 | |
joe4465 | 0:0010a5abcc31 | 128 | //USB serial |
joe4465 | 3:82665e39f1ea | 129 | Serial _wiredSerial(USBTX, USBRX); |
joe4465 | 0:0010a5abcc31 | 130 | |
joe4465 | 0:0010a5abcc31 | 131 | //Wireless Serial |
joe4465 | 3:82665e39f1ea | 132 | Serial _wirelessSerial(p9, p10); |
joe4465 | 0:0010a5abcc31 | 133 | |
joe4465 | 3:82665e39f1ea | 134 | //GPS Serial |
joe4465 | 6:4c207e7b1203 | 135 | //Serial _gpsSerial(p28, p27); |
joe4465 | 3:82665e39f1ea | 136 | |
joe4465 | 3:82665e39f1ea | 137 | //PPM in |
joe4465 | 3:82665e39f1ea | 138 | PPM *_ppm; |
joe4465 | 6:4c207e7b1203 | 139 | InterruptIn *_interruptPin = new InterruptIn(p7); |
joe4465 | 0:0010a5abcc31 | 140 | |
joe4465 | 0:0010a5abcc31 | 141 | //Battery monitor |
joe4465 | 6:4c207e7b1203 | 142 | //DigitalIn _batteryMonitor(p8); |
joe4465 | 0:0010a5abcc31 | 143 | |
joe4465 | 0:0010a5abcc31 | 144 | //Onboard LED's |
joe4465 | 3:82665e39f1ea | 145 | DigitalOut _led1(LED1); |
joe4465 | 3:82665e39f1ea | 146 | DigitalOut _led2(LED2); |
joe4465 | 3:82665e39f1ea | 147 | DigitalOut _led3(LED3); |
joe4465 | 3:82665e39f1ea | 148 | DigitalOut _led4(LED4); |
joe4465 | 0:0010a5abcc31 | 149 | |
joe4465 | 3:82665e39f1ea | 150 | //Outputs |
joe4465 | 3:82665e39f1ea | 151 | DigitalOut _output1(p11); |
joe4465 | 3:82665e39f1ea | 152 | DigitalOut _output2(p12); |
joe4465 | 3:82665e39f1ea | 153 | DigitalOut _output3(p5); |
joe4465 | 3:82665e39f1ea | 154 | DigitalOut _output4(p6); |
joe4465 | 0:0010a5abcc31 | 155 | |
joe4465 | 0:0010a5abcc31 | 156 | //IMU |
joe4465 | 3:82665e39f1ea | 157 | FreeIMU _freeIMU; |
joe4465 | 3:82665e39f1ea | 158 | |
joe4465 | 3:82665e39f1ea | 159 | //Functions/////////////////////////////////////////////////////////////////////////////////////////////// |
joe4465 | 6:4c207e7b1203 | 160 | //Zero gyro and arm |
joe4465 | 3:82665e39f1ea | 161 | void Arm() |
joe4465 | 3:82665e39f1ea | 162 | { |
joe4465 | 6:4c207e7b1203 | 163 | //Don't arm unless throttle is equal to 0 and the transmitter is connected |
joe4465 | 6:4c207e7b1203 | 164 | if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.1) && _rcMappedCommands[3] != -1) |
joe4465 | 6:4c207e7b1203 | 165 | { |
joe4465 | 6:4c207e7b1203 | 166 | //Zero gyro |
joe4465 | 6:4c207e7b1203 | 167 | _freeIMU.zeroGyro(); |
joe4465 | 6:4c207e7b1203 | 168 | |
joe4465 | 6:4c207e7b1203 | 169 | //Set armed to true |
joe4465 | 6:4c207e7b1203 | 170 | _armed = true; |
joe4465 | 6:4c207e7b1203 | 171 | } |
joe4465 | 3:82665e39f1ea | 172 | } |
joe4465 | 3:82665e39f1ea | 173 | |
joe4465 | 6:4c207e7b1203 | 174 | //Disarm |
joe4465 | 3:82665e39f1ea | 175 | void Disarm() |
joe4465 | 6:4c207e7b1203 | 176 | { |
joe4465 | 6:4c207e7b1203 | 177 | //Set armed to false |
joe4465 | 6:4c207e7b1203 | 178 | _armed = false; |
joe4465 | 3:82665e39f1ea | 179 | |
joe4465 | 6:4c207e7b1203 | 180 | //Disable modes |
joe4465 | 6:4c207e7b1203 | 181 | _levelOffset = false; |
joe4465 | 6:4c207e7b1203 | 182 | |
joe4465 | 6:4c207e7b1203 | 183 | //Save settings |
joe4465 | 6:4c207e7b1203 | 184 | WriteSettingsToConfig(); |
joe4465 | 3:82665e39f1ea | 185 | } |
joe4465 | 3:82665e39f1ea | 186 | |
joe4465 | 3:82665e39f1ea | 187 | //Zero pitch and roll |
joe4465 | 6:4c207e7b1203 | 188 | /*void ZeroPitchRoll() |
joe4465 | 3:82665e39f1ea | 189 | { |
joe4465 | 6:4c207e7b1203 | 190 | printf("Zeroing pitch and roll\r\n"); |
joe4465 | 6:4c207e7b1203 | 191 | |
joe4465 | 3:82665e39f1ea | 192 | //Zero pitch and roll |
joe4465 | 3:82665e39f1ea | 193 | float totalPitch = 0; |
joe4465 | 3:82665e39f1ea | 194 | float totalRoll = 0; |
joe4465 | 3:82665e39f1ea | 195 | float ypr[3] = {0,0,0}; // Yaw, pitch, roll |
joe4465 | 3:82665e39f1ea | 196 | for(int i = 0; i < 500; i++) |
joe4465 | 3:82665e39f1ea | 197 | { |
joe4465 | 3:82665e39f1ea | 198 | _freeIMU.getYawPitchRoll(ypr); |
joe4465 | 3:82665e39f1ea | 199 | totalPitch += ypr[1]; |
joe4465 | 3:82665e39f1ea | 200 | totalRoll += ypr[2]; |
joe4465 | 3:82665e39f1ea | 201 | } |
joe4465 | 3:82665e39f1ea | 202 | |
joe4465 | 3:82665e39f1ea | 203 | _zeroValues[1] = totalPitch/500; |
joe4465 | 3:82665e39f1ea | 204 | _zeroValues[2] = totalRoll/500; |
joe4465 | 3:82665e39f1ea | 205 | printf("Pitch %f\r\n", _zeroValues[1]); |
joe4465 | 3:82665e39f1ea | 206 | printf("Roll %f\r\n", _zeroValues[2]); |
joe4465 | 6:4c207e7b1203 | 207 | } */ |
joe4465 | 6:4c207e7b1203 | 208 | |
joe4465 | 6:4c207e7b1203 | 209 | //Saves settings to config file |
joe4465 | 6:4c207e7b1203 | 210 | void WriteSettingsToConfig() |
joe4465 | 6:4c207e7b1203 | 211 | { |
joe4465 | 6:4c207e7b1203 | 212 | _wiredSerial.printf("Writing settings to config file\n\r"); |
joe4465 | 6:4c207e7b1203 | 213 | |
joe4465 | 6:4c207e7b1203 | 214 | if(_armed == false) //Not flying |
joe4465 | 6:4c207e7b1203 | 215 | { |
joe4465 | 6:4c207e7b1203 | 216 | _freeIMU.sample(false); |
joe4465 | 6:4c207e7b1203 | 217 | |
joe4465 | 6:4c207e7b1203 | 218 | //Write values |
joe4465 | 6:4c207e7b1203 | 219 | ConvertToCharArray(_yawRatePIDControllerP); |
joe4465 | 6:4c207e7b1203 | 220 | if (!_configFile.setValue("_yawRatePIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 221 | { |
joe4465 | 6:4c207e7b1203 | 222 | _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 223 | } |
joe4465 | 6:4c207e7b1203 | 224 | |
joe4465 | 6:4c207e7b1203 | 225 | ConvertToCharArray(_yawRatePIDControllerI); |
joe4465 | 6:4c207e7b1203 | 226 | if (!_configFile.setValue("_yawRatePIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 227 | { |
joe4465 | 6:4c207e7b1203 | 228 | _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 229 | } |
joe4465 | 6:4c207e7b1203 | 230 | |
joe4465 | 6:4c207e7b1203 | 231 | ConvertToCharArray(_yawRatePIDControllerD); |
joe4465 | 6:4c207e7b1203 | 232 | if (!_configFile.setValue("_yawRatePIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 233 | { |
joe4465 | 6:4c207e7b1203 | 234 | _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 235 | } |
joe4465 | 6:4c207e7b1203 | 236 | |
joe4465 | 6:4c207e7b1203 | 237 | ConvertToCharArray(_pitchRatePIDControllerP); |
joe4465 | 6:4c207e7b1203 | 238 | if (!_configFile.setValue("_pitchRatePIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 239 | { |
joe4465 | 6:4c207e7b1203 | 240 | _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 241 | } |
joe4465 | 6:4c207e7b1203 | 242 | |
joe4465 | 6:4c207e7b1203 | 243 | ConvertToCharArray(_pitchRatePIDControllerI); |
joe4465 | 6:4c207e7b1203 | 244 | if (!_configFile.setValue("_pitchRatePIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 245 | { |
joe4465 | 6:4c207e7b1203 | 246 | _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 247 | } |
joe4465 | 6:4c207e7b1203 | 248 | |
joe4465 | 6:4c207e7b1203 | 249 | ConvertToCharArray(_pitchRatePIDControllerD); |
joe4465 | 6:4c207e7b1203 | 250 | if (!_configFile.setValue("_pitchRatePIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 251 | { |
joe4465 | 6:4c207e7b1203 | 252 | _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 253 | } |
joe4465 | 6:4c207e7b1203 | 254 | |
joe4465 | 6:4c207e7b1203 | 255 | ConvertToCharArray(_rollRatePIDControllerP); |
joe4465 | 6:4c207e7b1203 | 256 | if (!_configFile.setValue("_rollRatePIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 257 | { |
joe4465 | 6:4c207e7b1203 | 258 | _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 259 | } |
joe4465 | 6:4c207e7b1203 | 260 | |
joe4465 | 6:4c207e7b1203 | 261 | ConvertToCharArray(_rollRatePIDControllerI); |
joe4465 | 6:4c207e7b1203 | 262 | if (!_configFile.setValue("_rollRatePIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 263 | { |
joe4465 | 6:4c207e7b1203 | 264 | _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 265 | } |
joe4465 | 6:4c207e7b1203 | 266 | |
joe4465 | 6:4c207e7b1203 | 267 | ConvertToCharArray(_rollRatePIDControllerD); |
joe4465 | 6:4c207e7b1203 | 268 | if (!_configFile.setValue("_rollRatePIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 269 | { |
joe4465 | 6:4c207e7b1203 | 270 | _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 271 | } |
joe4465 | 6:4c207e7b1203 | 272 | |
joe4465 | 6:4c207e7b1203 | 273 | ConvertToCharArray(_yawStabPIDControllerP); |
joe4465 | 6:4c207e7b1203 | 274 | if (!_configFile.setValue("_yawStabPIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 275 | { |
joe4465 | 6:4c207e7b1203 | 276 | _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 277 | } |
joe4465 | 6:4c207e7b1203 | 278 | |
joe4465 | 6:4c207e7b1203 | 279 | ConvertToCharArray(_yawStabPIDControllerI); |
joe4465 | 6:4c207e7b1203 | 280 | if (!_configFile.setValue("_yawStabPIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 281 | { |
joe4465 | 6:4c207e7b1203 | 282 | _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 283 | } |
joe4465 | 6:4c207e7b1203 | 284 | |
joe4465 | 6:4c207e7b1203 | 285 | ConvertToCharArray(_yawStabPIDControllerD); |
joe4465 | 6:4c207e7b1203 | 286 | if (!_configFile.setValue("_yawStabPIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 287 | { |
joe4465 | 6:4c207e7b1203 | 288 | _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 289 | } |
joe4465 | 6:4c207e7b1203 | 290 | |
joe4465 | 6:4c207e7b1203 | 291 | ConvertToCharArray(_pitchStabPIDControllerP); |
joe4465 | 6:4c207e7b1203 | 292 | if (!_configFile.setValue("_pitchStabPIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 293 | { |
joe4465 | 6:4c207e7b1203 | 294 | _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 295 | } |
joe4465 | 6:4c207e7b1203 | 296 | |
joe4465 | 6:4c207e7b1203 | 297 | ConvertToCharArray(_pitchStabPIDControllerI); |
joe4465 | 6:4c207e7b1203 | 298 | if (!_configFile.setValue("_pitchStabPIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 299 | { |
joe4465 | 6:4c207e7b1203 | 300 | _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 301 | } |
joe4465 | 6:4c207e7b1203 | 302 | |
joe4465 | 6:4c207e7b1203 | 303 | ConvertToCharArray(_pitchStabPIDControllerD); |
joe4465 | 6:4c207e7b1203 | 304 | if (!_configFile.setValue("_pitchStabPIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 305 | { |
joe4465 | 6:4c207e7b1203 | 306 | _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 307 | } |
joe4465 | 6:4c207e7b1203 | 308 | |
joe4465 | 6:4c207e7b1203 | 309 | ConvertToCharArray(_rollStabPIDControllerP); |
joe4465 | 6:4c207e7b1203 | 310 | if (!_configFile.setValue("_rollStabPIDControllerP", _str)) |
joe4465 | 6:4c207e7b1203 | 311 | { |
joe4465 | 6:4c207e7b1203 | 312 | _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r"); |
joe4465 | 6:4c207e7b1203 | 313 | } |
joe4465 | 6:4c207e7b1203 | 314 | |
joe4465 | 6:4c207e7b1203 | 315 | ConvertToCharArray(_rollStabPIDControllerI); |
joe4465 | 6:4c207e7b1203 | 316 | if (!_configFile.setValue("_rollStabPIDControllerI", _str)) |
joe4465 | 6:4c207e7b1203 | 317 | { |
joe4465 | 6:4c207e7b1203 | 318 | _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r"); |
joe4465 | 6:4c207e7b1203 | 319 | } |
joe4465 | 6:4c207e7b1203 | 320 | |
joe4465 | 6:4c207e7b1203 | 321 | ConvertToCharArray(_rollStabPIDControllerD); |
joe4465 | 6:4c207e7b1203 | 322 | if (!_configFile.setValue("_rollStabPIDControllerD", _str)) |
joe4465 | 6:4c207e7b1203 | 323 | { |
joe4465 | 6:4c207e7b1203 | 324 | _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r"); |
joe4465 | 6:4c207e7b1203 | 325 | } |
joe4465 | 6:4c207e7b1203 | 326 | |
joe4465 | 6:4c207e7b1203 | 327 | ConvertToCharArray(_zeroValues[1]); |
joe4465 | 6:4c207e7b1203 | 328 | if (!_configFile.setValue("_zeroPitch", _str)) |
joe4465 | 6:4c207e7b1203 | 329 | { |
joe4465 | 6:4c207e7b1203 | 330 | _wiredSerial.printf("Failed to write value for zero pitch\n\r"); |
joe4465 | 6:4c207e7b1203 | 331 | } |
joe4465 | 6:4c207e7b1203 | 332 | |
joe4465 | 6:4c207e7b1203 | 333 | ConvertToCharArray(_zeroValues[2]); |
joe4465 | 6:4c207e7b1203 | 334 | if (!_configFile.setValue("_zeroRoll", _str)) |
joe4465 | 6:4c207e7b1203 | 335 | { |
joe4465 | 6:4c207e7b1203 | 336 | _wiredSerial.printf("Failed to write value for zero roll\n\r"); |
joe4465 | 6:4c207e7b1203 | 337 | } |
joe4465 | 6:4c207e7b1203 | 338 | |
joe4465 | 6:4c207e7b1203 | 339 | if (!_configFile.write("/local/config.cfg")) |
joe4465 | 6:4c207e7b1203 | 340 | { |
joe4465 | 6:4c207e7b1203 | 341 | _wiredSerial.printf("Failure to write settings to configuration file.\n\r"); |
joe4465 | 6:4c207e7b1203 | 342 | } |
joe4465 | 6:4c207e7b1203 | 343 | else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r"); |
joe4465 | 6:4c207e7b1203 | 344 | |
joe4465 | 6:4c207e7b1203 | 345 | _freeIMU.sample(true); |
joe4465 | 6:4c207e7b1203 | 346 | } |
joe4465 | 6:4c207e7b1203 | 347 | else |
joe4465 | 6:4c207e7b1203 | 348 | { |
joe4465 | 6:4c207e7b1203 | 349 | _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r"); |
joe4465 | 6:4c207e7b1203 | 350 | } |
joe4465 | 6:4c207e7b1203 | 351 | } |
joe4465 | 6:4c207e7b1203 | 352 | |
joe4465 | 6:4c207e7b1203 | 353 | //Converts float to char array |
joe4465 | 6:4c207e7b1203 | 354 | void ConvertToCharArray(float number) |
joe4465 | 6:4c207e7b1203 | 355 | { |
joe4465 | 6:4c207e7b1203 | 356 | sprintf(_str, "%1.8f", number ); |
joe4465 | 6:4c207e7b1203 | 357 | } |
joe4465 | 6:4c207e7b1203 | 358 | |
joe4465 | 6:4c207e7b1203 | 359 | //Converts integer to char array |
joe4465 | 6:4c207e7b1203 | 360 | void ConvertToCharArray(int number) |
joe4465 | 6:4c207e7b1203 | 361 | { |
joe4465 | 6:4c207e7b1203 | 362 | sprintf(_str, "%d", number ); |
joe4465 | 3:82665e39f1ea | 363 | } |
joe4465 | 3:82665e39f1ea | 364 | |
joe4465 | 3:82665e39f1ea | 365 | float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax) |
joe4465 | 3:82665e39f1ea | 366 | { |
joe4465 | 3:82665e39f1ea | 367 | return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin; |
joe4465 | 6:4c207e7b1203 | 368 | } |
joe4465 | 0:0010a5abcc31 | 369 | |
joe4465 | 0:0010a5abcc31 | 370 | #endif |