hardware.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "FreeIMU.h"
00004 #include "PID.h"
00005 #include "ConfigFile.h"
00006 #include "PPM.h"
00007 #include <sstream>
00008 #include <TinyGPS.h>
00009 #include "sonar.h"
00010 #include "MODSERIAL.h"
00011 #include "filter.h"
00012 
00013 #ifndef HARDWARE_H
00014 #define HARDWARE_H
00015 
00016 //Global constants
00017 #define             IMU_YAW_ANGLE_MAX 180
00018 #define             IMU_YAW_ANGLE_MIN -180
00019 #define             IMU_ROLL_ANGLE_MAX 90
00020 #define             IMU_ROLL_ANGLE_MIN -90
00021 #define             IMU_PITCH_ANGLE_MAX 90
00022 #define             IMU_PITCH_ANGLE_MIN -90
00023 #define             IMU_YAW_RATE_MAX 360
00024 #define             IMU_YAW_RATE_MIN -360
00025 #define             IMU_ROLL_RATE_MAX 360
00026 #define             IMU_ROLL_RATE_MIN -360
00027 #define             IMU_PITCH_RATE_MAX 360
00028 #define             IMU_PITCH_RATE_MIN -360
00029 
00030 #define             RC_CHANNELS 8
00031 #define             RC_THROTTLE_CHANNEL 3
00032 #define             RC_IN_MAX 1900
00033 #define             RC_IN_MIN 1000
00034 #define             RC_OUT_MAX 1
00035 #define             RC_OUT_MIN 0
00036 #define             RC_YAW_RATE_MAX 180
00037 #define             RC_YAW_RATE_MIN -180
00038 #define             RC_ROLL_RATE_MAX 90
00039 #define             RC_ROLL_RATE_MIN -90
00040 #define             RC_PITCH_RATE_MAX 90
00041 #define             RC_PITCH_RATE_MIN -90
00042 #define             RC_ROLL_ANGLE_MAX 20
00043 #define             RC_ROLL_ANGLE_MIN -20
00044 #define             RC_PITCH_ANGLE_MAX 20
00045 #define             RC_PITCH_ANGLE_MIN -20
00046 #define             RC_THRUST_MAX 1
00047 #define             RC_THRUST_MIN 0
00048 
00049 #define             MOTORS_OFF 0
00050 #define             MOTORS_ARMED 1000
00051 #define             MOTORS_MIN 1060
00052 #define             MOTORS_MAX 1860
00053 
00054 #define             RATE_PID_CONTROLLER_OUTPUT_MAX 100
00055 #define             RATE_PID_CONTROLLER_OUTPUT_MIN -100
00056 
00057 #define             FLIGHT_CONTROLLER_FREQUENCY 500
00058 
00059 //Global Functions
00060 //void ZeroPitchRoll();
00061 void Arm();
00062 void Disarm();
00063 void WriteSettingsToConfig();
00064 void ConvertToCharArray(float number);
00065 void ConvertToCharArray(int number);
00066 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
00067 
00068 //Global Variables
00069 float               _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD;
00070 float               _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD;
00071 float               _zeroValues[3] = {0,0,0}; //Yaw, pitch, roll
00072 float               _oldZeroValues[3] = {0,0,0}; //Yaw, pitch, roll
00073 float               _rcCommands[8] = {0,0,0,0,0,0,0,0};
00074 float               _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
00075 double              _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
00076 bool                _gpsConnected = false;
00077 bool                _armed = false;
00078 bool                _rate = false;
00079 bool                _stab = true;
00080 bool                _initialised = false;
00081 float               _motorPower [4] = {0,0,0,0};
00082 float               _gyroRate[3] = {0,0,0}; // Yaw, Pitch, Roll
00083 float               _ypr[3] = {0,0,0}; // Yaw, pitch, roll
00084 float               _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
00085 float               _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
00086 bool                _levelOffset = false;
00087 int                 _commsMode = 0;
00088 int                 _batt = 0;
00089 float               _yawTarget = 0;
00090 float               _maxBotixPingAltitude = 0;
00091 float               _barometerAltitude = 0;
00092     
00093 //PID controllers
00094 PID                 *_yawRatePIDController;
00095 PID                 *_pitchRatePIDController;
00096 PID                 *_rollRatePIDController;
00097 PID                 *_yawStabPIDController;
00098 PID                 *_pitchStabPIDController;
00099 PID                 *_rollStabPIDController;
00100 
00101 //Threads
00102 Thread              *_statusThread;
00103 Thread              *_serialPortMonitorThread;
00104 Thread              *_flightControllerThread;
00105 Thread              *_rcCommandMonitorThread;
00106 Thread              *_altitudeMonitorThread;
00107 
00108 //Config file
00109 LocalFileSystem     local("local");
00110 ConfigFile          _configFile;
00111 char*               _str = new char[1024];
00112 
00113 //RC filters
00114 medianFilter        *_yawMedianFilter;
00115 medianFilter        *_pitchMedianFilter;
00116 medianFilter        *_rollMedianFilter;
00117 medianFilter        *_thrustMedianFilter;
00118 
00119 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
00120 // M1  M2
00121 //  \  /
00122 //   \/
00123 //   /\
00124 //  /  \
00125 // M3  M4
00126  
00127 //Motors
00128 PwmOut              _motor1(p21);
00129 PwmOut              _motor2(p22);
00130 PwmOut              _motor3(p23);
00131 PwmOut              _motor4(p24);
00132 
00133 //USB serial
00134 MODSERIAL           _wiredSerial(USBTX, USBRX);
00135 
00136 //Wireless Serial
00137 MODSERIAL           _wirelessSerial(p9, p10);
00138 
00139 //GPS Serial
00140 MODSERIAL           _gps(p13, p14);
00141 TinyGPS             _tinyGPS;
00142 
00143 //PPM in
00144 PPM                 *_ppm;
00145 InterruptIn         *_interruptPin = new InterruptIn(p8);
00146 
00147 //Onboard LED's
00148 DigitalOut          _led1(LED1);
00149 DigitalOut          _led2(LED2);
00150 DigitalOut          _led3(LED3);
00151 DigitalOut          _led4(LED4);
00152 
00153 //IMU
00154 FreeIMU             _freeIMU;
00155 
00156 //Buzzer
00157 DigitalOut          _buzzer(p20);
00158 
00159 //MaxBotix Ping sensor
00160 Timer               _maxBotixTimer;
00161 Sonar               _maxBotixSensor(p15, _maxBotixTimer);
00162 
00163 //Unused analog pins
00164 DigitalOut         _spare1(p16);
00165 DigitalOut         _spare2(p17);
00166 DigitalOut         _spare3(p18);
00167 DigitalOut         _spare4(p19);
00168 
00169 //Functions///////////////////////////////////////////////////////////////////////////////////////////////
00170 //Zero gyro and arm
00171 void Arm()
00172 {
00173     //Don't arm unless throttle is equal to 0 and the transmitter is connected
00174     if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.2) && _rcMappedCommands[3] != -1 && _armed == false)
00175     {
00176         //Zero gyro
00177         _freeIMU.zeroGyro();
00178         
00179         //Set armed to true
00180         _armed = true; 
00181     }  
00182 }
00183 
00184 //Disarm
00185 void Disarm()
00186 {   
00187     if(_armed == true)
00188     {
00189         //Set armed to false
00190         _armed = false;  
00191         
00192         //Disable modes
00193         _levelOffset = false; 
00194         
00195         //Save settings
00196         WriteSettingsToConfig();
00197     }
00198 }
00199 
00200 //Zero pitch and roll
00201 /*void ZeroPitchRoll()
00202 {  
00203     printf("Zeroing pitch and roll\r\n");
00204     
00205     //Zero pitch and roll
00206     float totalPitch = 0;
00207     float totalRoll = 0;
00208     float ypr[3] = {0,0,0}; // Yaw, pitch, roll
00209     for(int i = 0; i < 500; i++)
00210     {
00211         _freeIMU.getYawPitchRoll(ypr);
00212         totalPitch += ypr[1];
00213         totalRoll += ypr[2];
00214     }
00215     
00216     _zeroValues[1] = totalPitch/500;
00217     _zeroValues[2] = totalRoll/500;
00218     printf("Pitch %f\r\n", _zeroValues[1]);
00219     printf("Roll %f\r\n", _zeroValues[2]);
00220 }  */  
00221 
00222 //Saves settings to config file
00223 void WriteSettingsToConfig()
00224 {
00225     _wiredSerial.printf("Writing settings to config file\n\r");
00226     
00227     if(_armed == false) //Not flying
00228     {
00229         _freeIMU.sample(false);
00230         
00231         //Write values
00232         ConvertToCharArray(_yawRatePIDControllerP);
00233         if (!_configFile.setValue("_yawRatePIDControllerP", _str))
00234         {
00235             _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
00236         }
00237         
00238         ConvertToCharArray(_yawRatePIDControllerI);
00239         if (!_configFile.setValue("_yawRatePIDControllerI", _str))
00240         {
00241             _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
00242         }
00243         
00244         ConvertToCharArray(_yawRatePIDControllerD);
00245         if (!_configFile.setValue("_yawRatePIDControllerD", _str))
00246         {
00247             _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
00248         }
00249         
00250         ConvertToCharArray(_pitchRatePIDControllerP);
00251         if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
00252         {
00253             _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
00254         }
00255         
00256         ConvertToCharArray(_pitchRatePIDControllerI);
00257         if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
00258         {
00259             _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
00260         }
00261         
00262         ConvertToCharArray(_pitchRatePIDControllerD);
00263         if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
00264         {
00265             _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
00266         }
00267         
00268         ConvertToCharArray(_rollRatePIDControllerP);
00269         if (!_configFile.setValue("_rollRatePIDControllerP", _str))
00270         {
00271             _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
00272         }
00273         
00274         ConvertToCharArray(_rollRatePIDControllerI);
00275         if (!_configFile.setValue("_rollRatePIDControllerI", _str))
00276         {
00277             _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
00278         }
00279         
00280         ConvertToCharArray(_rollRatePIDControllerD);
00281         if (!_configFile.setValue("_rollRatePIDControllerD", _str))
00282         {
00283             _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
00284         }
00285     
00286         ConvertToCharArray(_yawStabPIDControllerP);
00287         if (!_configFile.setValue("_yawStabPIDControllerP", _str))
00288         {
00289             _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
00290         }
00291         
00292         ConvertToCharArray(_yawStabPIDControllerI);
00293         if (!_configFile.setValue("_yawStabPIDControllerI", _str))
00294         {
00295             _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
00296         }
00297         
00298         ConvertToCharArray(_yawStabPIDControllerD);
00299         if (!_configFile.setValue("_yawStabPIDControllerD", _str))
00300         {
00301             _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
00302         }
00303         
00304         ConvertToCharArray(_pitchStabPIDControllerP);
00305         if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
00306         {
00307             _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
00308         }
00309         
00310         ConvertToCharArray(_pitchStabPIDControllerI);
00311         if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
00312         {
00313             _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
00314         }
00315         
00316         ConvertToCharArray(_pitchStabPIDControllerD);
00317         if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
00318         {
00319             _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
00320         }
00321         
00322         ConvertToCharArray(_rollStabPIDControllerP);
00323         if (!_configFile.setValue("_rollStabPIDControllerP", _str))
00324         {
00325             _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
00326         }
00327         
00328         ConvertToCharArray(_rollStabPIDControllerI);
00329         if (!_configFile.setValue("_rollStabPIDControllerI", _str))
00330         {
00331             _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
00332         }
00333         
00334         ConvertToCharArray(_rollStabPIDControllerD);
00335         if (!_configFile.setValue("_rollStabPIDControllerD", _str))
00336         {
00337             _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
00338         }
00339     
00340         ConvertToCharArray(_zeroValues[1]);
00341         if (!_configFile.setValue("_zeroPitch", _str))
00342         {
00343             _wiredSerial.printf("Failed to write value for zero pitch\n\r");
00344         }
00345         
00346         ConvertToCharArray(_zeroValues[2]);
00347         if (!_configFile.setValue("_zeroRoll", _str))
00348         {
00349             _wiredSerial.printf("Failed to write value for zero roll\n\r");
00350         }
00351         
00352         if (!_configFile.write("/local/config.cfg"))
00353         {
00354             _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
00355         }
00356         else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
00357         
00358         _freeIMU.sample(true);
00359     }
00360     else
00361     {
00362         _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
00363     }
00364 }
00365 
00366 //Converts float to char array
00367 void ConvertToCharArray(float number)
00368 {
00369     sprintf(_str, "%1.8f", number );  
00370 }
00371 
00372 //Converts integer to char array
00373 void ConvertToCharArray(int number)
00374 {
00375     sprintf(_str, "%d", number );  
00376 }
00377 
00378 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
00379 {
00380     return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
00381 }
00382 
00383 #endif