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@9:7b194f83e567, 2015-02-22 (annotated)

Committer:
joe4465
Date:
Sun Feb 22 20:10:12 2015 +0000
Revision:
9:7b194f83e567
Parent:
7:bc5822aa8878 
Added external magnetometer

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