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

hardware.h@7:bc5822aa8878, 2015-02-10 (annotated)

Committer:
joe4465
Date:
Tue Feb 10 20:55:44 2015 +0000
Revision:
7:bc5822aa8878
Parent:
6:4c207e7b1203
Child:
9:7b194f83e567
Updated software to match new PCB

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 "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 7:bc5822aa8878 39 #define RC_ROLL_ANGLE_MAX 45
joe4465 7:bc5822aa8878 40 #define RC_ROLL_ANGLE_MIN -45
joe4465 7:bc5822aa8878 41 #define RC_PITCH_ANGLE_MAX 45
joe4465 7:bc5822aa8878 42 #define RC_PITCH_ANGLE_MIN -45
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 7:bc5822aa8878 107 //medianFilter *_yawMedianFilter;
joe4465 7:bc5822aa8878 108 //medianFilter *_pitchMedianFilter;
joe4465 7:bc5822aa8878 109 //medianFilter *_rollMedianFilter;
joe4465 7:bc5822aa8878 110 //medianFilter *_thrustMedianFilter;
joe4465 7:bc5822aa8878 111 //medianFilter *_channel7MedianFilter;
joe4465 7:bc5822aa8878 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 7:bc5822aa8878 123 PwmOut _motor1(p21);
joe4465 7:bc5822aa8878 124 PwmOut _motor2(p22);
joe4465 7:bc5822aa8878 125 PwmOut _motor3(p23);
joe4465 7:bc5822aa8878 126 PwmOut _motor4(p24);
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 7:bc5822aa8878 139 InterruptIn *_interruptPin = new InterruptIn(p8);
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 7:bc5822aa8878 151 //DigitalOut _output1(p11);
joe4465 7:bc5822aa8878 152 //DigitalOut _output2(p12);
joe4465 7:bc5822aa8878 153 //DigitalOut _output3(p5);
joe4465 7:bc5822aa8878 154 //DigitalOut _output4(p6);
joe4465 0:0010a5abcc31 155
joe4465 0:0010a5abcc31 156 //IMU
joe4465 3:82665e39f1ea 157 FreeIMU _freeIMU;
joe4465 3:82665e39f1ea 158
joe4465 7:bc5822aa8878 159 //Buzzer
joe4465 7:bc5822aa8878 160 DigitalOut _buzzer(p20);
joe4465 7:bc5822aa8878 161
joe4465 3:82665e39f1ea 162 //Functions///////////////////////////////////////////////////////////////////////////////////////////////
joe4465 6:4c207e7b1203 163 //Zero gyro and arm
joe4465 3:82665e39f1ea 164 void Arm()
joe4465 3:82665e39f1ea 165 {
joe4465 6:4c207e7b1203 166 //Don't arm unless throttle is equal to 0 and the transmitter is connected
joe4465 7:bc5822aa8878 167 if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.2) && _rcMappedCommands[3] != -1 && _armed == false)
joe4465 6:4c207e7b1203 168 {
joe4465 6:4c207e7b1203 169 //Zero gyro
joe4465 6:4c207e7b1203 170 _freeIMU.zeroGyro();
joe4465 6:4c207e7b1203 171
joe4465 6:4c207e7b1203 172 //Set armed to true
joe4465 6:4c207e7b1203 173 _armed = true;
joe4465 6:4c207e7b1203 174 }
joe4465 3:82665e39f1ea 175 }
joe4465 3:82665e39f1ea 176
joe4465 6:4c207e7b1203 177 //Disarm
joe4465 3:82665e39f1ea 178 void Disarm()
joe4465 6:4c207e7b1203 179 {
joe4465 7:bc5822aa8878 180 if(_armed == true)
joe4465 7:bc5822aa8878 181 {
joe4465 7:bc5822aa8878 182 //Set armed to false
joe4465 7:bc5822aa8878 183 _armed = false;
joe4465 7:bc5822aa8878 184
joe4465 7:bc5822aa8878 185 //Disable modes
joe4465 7:bc5822aa8878 186 _levelOffset = false;
joe4465 7:bc5822aa8878 187
joe4465 7:bc5822aa8878 188 //Save settings
joe4465 7:bc5822aa8878 189 WriteSettingsToConfig();
joe4465 7:bc5822aa8878 190 }
joe4465 3:82665e39f1ea 191 }
joe4465 3:82665e39f1ea 192
joe4465 3:82665e39f1ea 193 //Zero pitch and roll
joe4465 6:4c207e7b1203 194 /*void ZeroPitchRoll()
joe4465 3:82665e39f1ea 195 {
joe4465 6:4c207e7b1203 196 printf("Zeroing pitch and roll\r\n");
joe4465 6:4c207e7b1203 197
joe4465 3:82665e39f1ea 198 //Zero pitch and roll
joe4465 3:82665e39f1ea 199 float totalPitch = 0;
joe4465 3:82665e39f1ea 200 float totalRoll = 0;
joe4465 3:82665e39f1ea 201 float ypr[3] = {0,0,0}; // Yaw, pitch, roll
joe4465 3:82665e39f1ea 202 for(int i = 0; i < 500; i++)
joe4465 3:82665e39f1ea 203 {
joe4465 3:82665e39f1ea 204 _freeIMU.getYawPitchRoll(ypr);
joe4465 3:82665e39f1ea 205 totalPitch += ypr[1];
joe4465 3:82665e39f1ea 206 totalRoll += ypr[2];
joe4465 3:82665e39f1ea 207 }
joe4465 3:82665e39f1ea 208
joe4465 3:82665e39f1ea 209 _zeroValues[1] = totalPitch/500;
joe4465 3:82665e39f1ea 210 _zeroValues[2] = totalRoll/500;
joe4465 3:82665e39f1ea 211 printf("Pitch %f\r\n", _zeroValues[1]);
joe4465 3:82665e39f1ea 212 printf("Roll %f\r\n", _zeroValues[2]);
joe4465 6:4c207e7b1203 213 } */
joe4465 6:4c207e7b1203 214
joe4465 6:4c207e7b1203 215 //Saves settings to config file
joe4465 6:4c207e7b1203 216 void WriteSettingsToConfig()
joe4465 6:4c207e7b1203 217 {
joe4465 6:4c207e7b1203 218 _wiredSerial.printf("Writing settings to config file\n\r");
joe4465 6:4c207e7b1203 219
joe4465 6:4c207e7b1203 220 if(_armed == false) //Not flying
joe4465 6:4c207e7b1203 221 {
joe4465 6:4c207e7b1203 222 _freeIMU.sample(false);
joe4465 6:4c207e7b1203 223
joe4465 6:4c207e7b1203 224 //Write values
joe4465 6:4c207e7b1203 225 ConvertToCharArray(_yawRatePIDControllerP);
joe4465 6:4c207e7b1203 226 if (!_configFile.setValue("_yawRatePIDControllerP", _str))
joe4465 6:4c207e7b1203 227 {
joe4465 6:4c207e7b1203 228 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 229 }
joe4465 6:4c207e7b1203 230
joe4465 6:4c207e7b1203 231 ConvertToCharArray(_yawRatePIDControllerI);
joe4465 6:4c207e7b1203 232 if (!_configFile.setValue("_yawRatePIDControllerI", _str))
joe4465 6:4c207e7b1203 233 {
joe4465 6:4c207e7b1203 234 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 235 }
joe4465 6:4c207e7b1203 236
joe4465 6:4c207e7b1203 237 ConvertToCharArray(_yawRatePIDControllerD);
joe4465 6:4c207e7b1203 238 if (!_configFile.setValue("_yawRatePIDControllerD", _str))
joe4465 6:4c207e7b1203 239 {
joe4465 6:4c207e7b1203 240 _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 241 }
joe4465 6:4c207e7b1203 242
joe4465 6:4c207e7b1203 243 ConvertToCharArray(_pitchRatePIDControllerP);
joe4465 6:4c207e7b1203 244 if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
joe4465 6:4c207e7b1203 245 {
joe4465 6:4c207e7b1203 246 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 247 }
joe4465 6:4c207e7b1203 248
joe4465 6:4c207e7b1203 249 ConvertToCharArray(_pitchRatePIDControllerI);
joe4465 6:4c207e7b1203 250 if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
joe4465 6:4c207e7b1203 251 {
joe4465 6:4c207e7b1203 252 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 253 }
joe4465 6:4c207e7b1203 254
joe4465 6:4c207e7b1203 255 ConvertToCharArray(_pitchRatePIDControllerD);
joe4465 6:4c207e7b1203 256 if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
joe4465 6:4c207e7b1203 257 {
joe4465 6:4c207e7b1203 258 _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 259 }
joe4465 6:4c207e7b1203 260
joe4465 6:4c207e7b1203 261 ConvertToCharArray(_rollRatePIDControllerP);
joe4465 6:4c207e7b1203 262 if (!_configFile.setValue("_rollRatePIDControllerP", _str))
joe4465 6:4c207e7b1203 263 {
joe4465 6:4c207e7b1203 264 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
joe4465 6:4c207e7b1203 265 }
joe4465 6:4c207e7b1203 266
joe4465 6:4c207e7b1203 267 ConvertToCharArray(_rollRatePIDControllerI);
joe4465 6:4c207e7b1203 268 if (!_configFile.setValue("_rollRatePIDControllerI", _str))
joe4465 6:4c207e7b1203 269 {
joe4465 6:4c207e7b1203 270 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
joe4465 6:4c207e7b1203 271 }
joe4465 6:4c207e7b1203 272
joe4465 6:4c207e7b1203 273 ConvertToCharArray(_rollRatePIDControllerD);
joe4465 6:4c207e7b1203 274 if (!_configFile.setValue("_rollRatePIDControllerD", _str))
joe4465 6:4c207e7b1203 275 {
joe4465 6:4c207e7b1203 276 _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
joe4465 6:4c207e7b1203 277 }
joe4465 6:4c207e7b1203 278
joe4465 6:4c207e7b1203 279 ConvertToCharArray(_yawStabPIDControllerP);
joe4465 6:4c207e7b1203 280 if (!_configFile.setValue("_yawStabPIDControllerP", _str))
joe4465 6:4c207e7b1203 281 {
joe4465 6:4c207e7b1203 282 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 283 }
joe4465 6:4c207e7b1203 284
joe4465 6:4c207e7b1203 285 ConvertToCharArray(_yawStabPIDControllerI);
joe4465 6:4c207e7b1203 286 if (!_configFile.setValue("_yawStabPIDControllerI", _str))
joe4465 6:4c207e7b1203 287 {
joe4465 6:4c207e7b1203 288 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 289 }
joe4465 6:4c207e7b1203 290
joe4465 6:4c207e7b1203 291 ConvertToCharArray(_yawStabPIDControllerD);
joe4465 6:4c207e7b1203 292 if (!_configFile.setValue("_yawStabPIDControllerD", _str))
joe4465 6:4c207e7b1203 293 {
joe4465 6:4c207e7b1203 294 _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 295 }
joe4465 6:4c207e7b1203 296
joe4465 6:4c207e7b1203 297 ConvertToCharArray(_pitchStabPIDControllerP);
joe4465 6:4c207e7b1203 298 if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
joe4465 6:4c207e7b1203 299 {
joe4465 6:4c207e7b1203 300 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 301 }
joe4465 6:4c207e7b1203 302
joe4465 6:4c207e7b1203 303 ConvertToCharArray(_pitchStabPIDControllerI);
joe4465 6:4c207e7b1203 304 if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
joe4465 6:4c207e7b1203 305 {
joe4465 6:4c207e7b1203 306 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 307 }
joe4465 6:4c207e7b1203 308
joe4465 6:4c207e7b1203 309 ConvertToCharArray(_pitchStabPIDControllerD);
joe4465 6:4c207e7b1203 310 if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
joe4465 6:4c207e7b1203 311 {
joe4465 6:4c207e7b1203 312 _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 313 }
joe4465 6:4c207e7b1203 314
joe4465 6:4c207e7b1203 315 ConvertToCharArray(_rollStabPIDControllerP);
joe4465 6:4c207e7b1203 316 if (!_configFile.setValue("_rollStabPIDControllerP", _str))
joe4465 6:4c207e7b1203 317 {
joe4465 6:4c207e7b1203 318 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
joe4465 6:4c207e7b1203 319 }
joe4465 6:4c207e7b1203 320
joe4465 6:4c207e7b1203 321 ConvertToCharArray(_rollStabPIDControllerI);
joe4465 6:4c207e7b1203 322 if (!_configFile.setValue("_rollStabPIDControllerI", _str))
joe4465 6:4c207e7b1203 323 {
joe4465 6:4c207e7b1203 324 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
joe4465 6:4c207e7b1203 325 }
joe4465 6:4c207e7b1203 326
joe4465 6:4c207e7b1203 327 ConvertToCharArray(_rollStabPIDControllerD);
joe4465 6:4c207e7b1203 328 if (!_configFile.setValue("_rollStabPIDControllerD", _str))
joe4465 6:4c207e7b1203 329 {
joe4465 6:4c207e7b1203 330 _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
joe4465 6:4c207e7b1203 331 }
joe4465 6:4c207e7b1203 332
joe4465 6:4c207e7b1203 333 ConvertToCharArray(_zeroValues[1]);
joe4465 6:4c207e7b1203 334 if (!_configFile.setValue("_zeroPitch", _str))
joe4465 6:4c207e7b1203 335 {
joe4465 6:4c207e7b1203 336 _wiredSerial.printf("Failed to write value for zero pitch\n\r");
joe4465 6:4c207e7b1203 337 }
joe4465 6:4c207e7b1203 338
joe4465 6:4c207e7b1203 339 ConvertToCharArray(_zeroValues[2]);
joe4465 6:4c207e7b1203 340 if (!_configFile.setValue("_zeroRoll", _str))
joe4465 6:4c207e7b1203 341 {
joe4465 6:4c207e7b1203 342 _wiredSerial.printf("Failed to write value for zero roll\n\r");
joe4465 6:4c207e7b1203 343 }
joe4465 6:4c207e7b1203 344
joe4465 6:4c207e7b1203 345 if (!_configFile.write("/local/config.cfg"))
joe4465 6:4c207e7b1203 346 {
joe4465 6:4c207e7b1203 347 _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
joe4465 6:4c207e7b1203 348 }
joe4465 6:4c207e7b1203 349 else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
joe4465 6:4c207e7b1203 350
joe4465 6:4c207e7b1203 351 _freeIMU.sample(true);
joe4465 6:4c207e7b1203 352 }
joe4465 6:4c207e7b1203 353 else
joe4465 6:4c207e7b1203 354 {
joe4465 6:4c207e7b1203 355 _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
joe4465 6:4c207e7b1203 356 }
joe4465 6:4c207e7b1203 357 }
joe4465 6:4c207e7b1203 358
joe4465 6:4c207e7b1203 359 //Converts float to char array
joe4465 6:4c207e7b1203 360 void ConvertToCharArray(float number)
joe4465 6:4c207e7b1203 361 {
joe4465 6:4c207e7b1203 362 sprintf(_str, "%1.8f", number );
joe4465 6:4c207e7b1203 363 }
joe4465 6:4c207e7b1203 364
joe4465 6:4c207e7b1203 365 //Converts integer to char array
joe4465 6:4c207e7b1203 366 void ConvertToCharArray(int number)
joe4465 6:4c207e7b1203 367 {
joe4465 6:4c207e7b1203 368 sprintf(_str, "%d", number );
joe4465 3:82665e39f1ea 369 }
joe4465 3:82665e39f1ea 370
joe4465 3:82665e39f1ea 371 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
joe4465 3:82665e39f1ea 372 {
joe4465 3:82665e39f1ea 373 return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
joe4465 6:4c207e7b1203 374 }
joe4465 0:0010a5abcc31 375
joe4465 0:0010a5abcc31 376 #endif