Diff: hardware.h

Revision:

6:4c207e7b1203

Parent:

3:82665e39f1ea

Child:

7:bc5822aa8878

--- a/hardware.h	Mon Sep 22 10:16:31 2014 +0000
+++ b/hardware.h	Thu Jan 22 18:03:22 2015 +0000
@@ -3,14 +3,14 @@
 #include "FreeIMU.h"
 #include "PID.h"
 #include "ConfigFile.h"
-#include "beep.h"
 #include "PPM.h"
+#include "filter.h"
 #include <sstream>
 
 #ifndef HARDWARE_H
 #define HARDWARE_H
 
-//Constants
+//Global constants
 #define             IMU_YAW_ANGLE_MAX 180
 #define             IMU_YAW_ANGLE_MIN -180
 #define             IMU_ROLL_ANGLE_MAX 90
@@ -30,16 +30,16 @@
 #define             RC_IN_MIN 1000
 #define             RC_OUT_MAX 1
 #define             RC_OUT_MIN 0
-#define             RC_YAW_RATE_MAX 180
-#define             RC_YAW_RATE_MIN -180
-#define             RC_ROLL_RATE_MAX 180
-#define             RC_ROLL_RATE_MIN -180
-#define             RC_PITCH_RATE_MAX 180
-#define             RC_PITCH_RATE_MIN -180
-#define             RC_ROLL_ANGLE_MAX 45
-#define             RC_ROLL_ANGLE_MIN -45
-#define             RC_PITCH_ANGLE_MAX 45
-#define             RC_PITCH_ANGLE_MIN -45
+#define             RC_YAW_RATE_MAX 90
+#define             RC_YAW_RATE_MIN -90
+#define             RC_ROLL_RATE_MAX 90
+#define             RC_ROLL_RATE_MIN -90
+#define             RC_PITCH_RATE_MAX 90
+#define             RC_PITCH_RATE_MIN -90
+#define             RC_ROLL_ANGLE_MAX 15
+#define             RC_ROLL_ANGLE_MIN -15
+#define             RC_PITCH_ANGLE_MAX 15
+#define             RC_PITCH_ANGLE_MIN -15
 #define             RC_THRUST_MAX 1
 #define             RC_THRUST_MIN 0
 
@@ -52,35 +52,37 @@
 #define             RATE_PID_CONTROLLER_OUTPUT_MIN -100
 
 #define             FLIGHT_CONTROLLER_FREQUENCY 500
-#define             MOTOR_PWM_FREQUENCY 500
-#define             RC_COMMANDS_FREQUENCY 50
-#define             SERIAL_MONITOR_FREQUENCY 10
-#define             PING_FREQUENCY 10
-#define             PIXY_CAM_FREQUENCY 10
-#define             STATUS_LIGHTS_FREQUENCY 10
 
-//Shared Functions
-void ZeroPitchRoll();
+//Global Functions
+//void ZeroPitchRoll();
 void Arm();
 void Disarm();
+void WriteSettingsToConfig();
+void ConvertToCharArray(float number);
+void ConvertToCharArray(int number);
 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
 
-//Shared Variables
+//Global Variables
 float               _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD;
 float               _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD;
 float               _zeroValues[3] = {0,0,0}; //Yaw, pitch, roll
+float               _oldZeroValues[3] = {0,0,0}; //Yaw, pitch, roll
+float               _rcCommands[8] = {0,0,0,0,0,0,0,0};
 float               _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
-//float               _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
+float               _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
 bool                _armed = false;
 bool                _rate = false;
 bool                _stab = true;
-bool                _initialised = true;
-//bool                _gpsConnected = false;
+bool                _initialised = false;
+bool                _gpsConnected = false;
 float               _motorPower [4] = {0,0,0,0};
-float               _gyroRate[3] ={}; // Yaw, Pitch, Roll
+float               _gyroRate[3] = {0,0,0}; // Yaw, Pitch, Roll
 float               _ypr[3] = {0,0,0}; // Yaw, pitch, roll
 float               _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
 float               _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
+bool                _levelOffset = false;
+int                 _commsMode = 0;
+int                 _batt = 0;
     
 //PID controllers
 PID                 *_yawRatePIDController;
@@ -99,6 +101,15 @@
 //Config file
 LocalFileSystem     local("local");
 ConfigFile          _configFile;
+char*               _str = new char[1024];
+
+//RC filters
+medianFilter        *_yawMedianFilter;
+medianFilter        *_pitchMedianFilter;
+medianFilter        *_rollMedianFilter;
+medianFilter        *_thrustMedianFilter;
+medianFilter        *_channel7MedianFilter;
+medianFilter        *_channel8MedianFilter;
 
 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
 // M1  M2
@@ -121,14 +132,14 @@
 Serial              _wirelessSerial(p9, p10);
 
 //GPS Serial
-//Serial              _gpsSerial(p28, p27);
+//Serial            _gpsSerial(p28, p27);
 
 //PPM in
 PPM *_ppm;
-InterruptIn *_interruptPin = new InterruptIn(p5);
+InterruptIn *_interruptPin = new InterruptIn(p7);
 
 //Battery monitor
-//DigitalIn           _batteryMonitor(p8);
+//DigitalIn         _batteryMonitor(p8);
 
 //Onboard LED's
 DigitalOut          _led1(LED1);
@@ -142,39 +153,42 @@
 DigitalOut          _output3(p5);
 DigitalOut          _output4(p6);
 
-//Buzzer
-Beep                _buzzer(p26);
-
 //IMU
 FreeIMU             _freeIMU;
 
 //Functions///////////////////////////////////////////////////////////////////////////////////////////////
-//Zero gyro, zero yaw and arm
+//Zero gyro and arm
 void Arm()
 {
-    //Zero gyro
-    _freeIMU.zeroGyro();
-    
-    //Set armed to true
-    _armed = true;   
+    //Don't arm unless throttle is equal to 0 and the transmitter is connected
+    if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.1) && _rcMappedCommands[3] != -1)
+    {
+        //Zero gyro
+        _freeIMU.zeroGyro();
+        
+        //Set armed to true
+        _armed = true; 
+    }  
 }
 
-//Set all RC to 0 and disarm
+//Disarm
 void Disarm()
-{
-    //Set all RC to 0
-    _rcMappedCommands[0] = 0;
-    _rcMappedCommands[1] = 0;
-    _rcMappedCommands[2] = 0;
-    _rcMappedCommands[3] = 0;
+{   
+    //Set armed to false
+    _armed = false;  
     
-    //Set armed to false
-    _armed = false;   
+    //Disable modes
+    _levelOffset = false; 
+    
+    //Save settings
+    WriteSettingsToConfig();
 }
 
 //Zero pitch and roll
-void ZeroPitchRoll()
+/*void ZeroPitchRoll()
 {  
+    printf("Zeroing pitch and roll\r\n");
+    
     //Zero pitch and roll
     float totalPitch = 0;
     float totalRoll = 0;
@@ -190,11 +204,167 @@
     _zeroValues[2] = totalRoll/500;
     printf("Pitch %f\r\n", _zeroValues[1]);
     printf("Roll %f\r\n", _zeroValues[2]);
+}  */  
+
+//Saves settings to config file
+void WriteSettingsToConfig()
+{
+    _wiredSerial.printf("Writing settings to config file\n\r");
+    
+    if(_armed == false) //Not flying
+    {
+        _freeIMU.sample(false);
+        
+        //Write values
+        ConvertToCharArray(_yawRatePIDControllerP);
+        if (!_configFile.setValue("_yawRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_yawRatePIDControllerI);
+        if (!_configFile.setValue("_yawRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_yawRatePIDControllerD);
+        if (!_configFile.setValue("_yawRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerP);
+        if (!_configFile.setValue("_pitchRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerI);
+        if (!_configFile.setValue("_pitchRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_pitchRatePIDControllerD);
+        if (!_configFile.setValue("_pitchRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerP);
+        if (!_configFile.setValue("_rollRatePIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerI);
+        if (!_configFile.setValue("_rollRatePIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_rollRatePIDControllerD);
+        if (!_configFile.setValue("_rollRatePIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r");
+        }
+    
+        ConvertToCharArray(_yawStabPIDControllerP);
+        if (!_configFile.setValue("_yawStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_yawStabPIDControllerI);
+        if (!_configFile.setValue("_yawStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_yawStabPIDControllerD);
+        if (!_configFile.setValue("_yawStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerP);
+        if (!_configFile.setValue("_pitchStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerI);
+        if (!_configFile.setValue("_pitchStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_pitchStabPIDControllerD);
+        if (!_configFile.setValue("_pitchStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerP);
+        if (!_configFile.setValue("_rollStabPIDControllerP", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerI);
+        if (!_configFile.setValue("_rollStabPIDControllerI", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r");
+        }
+        
+        ConvertToCharArray(_rollStabPIDControllerD);
+        if (!_configFile.setValue("_rollStabPIDControllerD", _str))
+        {
+            _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r");
+        }
+    
+        ConvertToCharArray(_zeroValues[1]);
+        if (!_configFile.setValue("_zeroPitch", _str))
+        {
+            _wiredSerial.printf("Failed to write value for zero pitch\n\r");
+        }
+        
+        ConvertToCharArray(_zeroValues[2]);
+        if (!_configFile.setValue("_zeroRoll", _str))
+        {
+            _wiredSerial.printf("Failed to write value for zero roll\n\r");
+        }
+        
+        if (!_configFile.write("/local/config.cfg"))
+        {
+            _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
+        }
+        else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r");
+        
+        _freeIMU.sample(true);
+    }
+    else
+    {
+        _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r");
+    }
+}
+
+//Converts float to char array
+void ConvertToCharArray(float number)
+{
+    sprintf(_str, "%1.8f", number );  
+}
+
+//Converts integer to char array
+void ConvertToCharArray(int number)
+{
+    sprintf(_str, "%d", number );  
 }
 
 float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
 {
     return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
-}    
+}
 
 #endif
\ No newline at end of file