Diff: hardware.h

Revision:

3:82665e39f1ea

Parent:

2:b3b771c8f7d1

Child:

6:4c207e7b1203

--- a/hardware.h	Fri May 16 14:22:18 2014 +0000
+++ b/hardware.h	Thu Sep 18 08:45:46 2014 +0000
@@ -4,73 +4,101 @@
 #include "PID.h"
 #include "ConfigFile.h"
 #include "beep.h"
+#include "PPM.h"
 #include <sstream>
 
 #ifndef HARDWARE_H
 #define HARDWARE_H
 
 //Constants
-#define IMU_YAW_ANGLE_MAX 180
-#define IMU_YAW_ANGLE_MIN -180
-#define IMU_ROLL_ANGLE_MAX 90
-#define IMU_ROLL_ANGLE_MIN -90
-#define IMU_PITCH_ANGLE_MAX 90
-#define IMU_PITCH_ANGLE_MIN -90
-#define IMU_YAW_RATE_MAX 360
-#define IMU_YAW_RATE_MIN -360
-#define IMU_ROLL_RATE_MAX 360
-#define IMU_ROLL_RATE_MIN -360
-#define IMU_PITCH_RATE_MAX 360
-#define IMU_PITCH_RATE_MIN -360
-#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 45
-#define RC_ROLL_ANGLE_MIN -45
-#define RC_PITCH_ANGLE_MAX 45
-#define RC_PITCH_ANGLE_MIN -45
-#define RC_THRUST_MAX 1
-#define RC_THRUST_MIN 0
-#define MOTORS_OFF 0
-#define MOTORS_MIN 1060
-#define MOTORS_MAX 1860
-#define MOTORS_ARMED 1000
-#define RATE_PID_CONTROLLER_OUTPUT_MAX 1
-#define RATE_PID_CONTROLLER_OUTPUT_MIN -1
-#define UPDATE_FREQUENCY 500
-#define MOTOR_PWM_FREQUENCY 500
-#define RCMIN -127
-#define RCMAX 127
+#define             IMU_YAW_ANGLE_MAX 180
+#define             IMU_YAW_ANGLE_MIN -180
+#define             IMU_ROLL_ANGLE_MAX 90
+#define             IMU_ROLL_ANGLE_MIN -90
+#define             IMU_PITCH_ANGLE_MAX 90
+#define             IMU_PITCH_ANGLE_MIN -90
+#define             IMU_YAW_RATE_MAX 360
+#define             IMU_YAW_RATE_MIN -360
+#define             IMU_ROLL_RATE_MAX 360
+#define             IMU_ROLL_RATE_MIN -360
+#define             IMU_PITCH_RATE_MAX 360
+#define             IMU_PITCH_RATE_MIN -360
+
+#define             RC_CHANNELS 8
+#define             RC_THROTTLE_CHANNEL 3
+#define             RC_IN_MAX 1900
+#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_THRUST_MAX 1
+#define             RC_THRUST_MIN 0
+
+#define             MOTORS_OFF 0
+#define             MOTORS_ARMED 1000
+#define             MOTORS_MIN 1060
+#define             MOTORS_MAX 1860
+
+#define             RATE_PID_CONTROLLER_OUTPUT_MAX 100
+#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();
+void Arm();
+void Disarm();
+float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
 
 //Shared 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 _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
-bool _armed = false;
-bool _rate = false;
-bool _stab = true;
-bool _initialised = true;
+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               _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
+//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;
+float               _motorPower [4] = {0,0,0,0};
+float               _gyroRate[3] ={}; // 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
+    
+//PID controllers
+PID                 *_yawRatePIDController;
+PID                 *_pitchRatePIDController;
+PID                 *_rollRatePIDController;
+PID                 *_yawStabPIDController;
+PID                 *_pitchStabPIDController;
+PID                 *_rollStabPIDController;
 
-//PID controllers
-PID *_yawRatePIDController;
-PID *_pitchRatePIDController;
-PID *_rollRatePIDController;
-PID *_yawStabPIDController;
-PID *_pitchStabPIDController;
-PID *_rollStabPIDController;
+//Threads
+Thread              *_statusThread;
+Thread              *_serialPortMonitorThread;
+Thread              *_flightControllerThread;
+Thread              *_rcCommandMonitorThread;
 
 //Config file
-LocalFileSystem local("local");
-ConfigFile _configFile;
-
-//Threads
-Thread *_statusThread;
-Thread *_serialPortMonitorThread;
-Thread *_flightControllerThread;
+LocalFileSystem     local("local");
+ConfigFile          _configFile;
 
 //HARDWARE////////////////////////////////////////////////////////////////////////////////////
 // M1  M2
@@ -81,39 +109,92 @@
 // M3  M4
  
 //Motors
-PwmOut _motor1(p22);
-PwmOut _motor2(p23);
-PwmOut _motor3(p24);
-PwmOut _motor4(p25);
+PwmOut              _motor1(p22);
+PwmOut              _motor2(p23);
+PwmOut              _motor3(p24);
+PwmOut              _motor4(p25);
 
 //USB serial
-Serial _wiredSerial(USBTX, USBRX); // tx, rx
+Serial              _wiredSerial(USBTX, USBRX);
 
 //Wireless Serial
-Serial _wirelessSerial(p9, p10);
+Serial              _wirelessSerial(p9, p10);
 
-//PPM
-InterruptIn _PPMSignal(p7);
+//GPS Serial
+//Serial              _gpsSerial(p28, p27);
+
+//PPM in
+PPM *_ppm;
+InterruptIn *_interruptPin = new InterruptIn(p5);
 
 //Battery monitor
-DigitalIn _batteryMonitor(p8);
+//DigitalIn           _batteryMonitor(p8);
 
 //Onboard LED's
-DigitalOut _led1(LED1);
-DigitalOut _led2(LED2);
-DigitalOut _led3(LED3);
-DigitalOut _led4(LED4);
+DigitalOut          _led1(LED1);
+DigitalOut          _led2(LED2);
+DigitalOut          _led3(LED3);
+DigitalOut          _led4(LED4);
 
-//External LED's
-DigitalOut _output1(p11);
-DigitalOut _output2(p12);
-DigitalOut _output3(p5);
-DigitalOut _output4(p6);
+//Outputs
+DigitalOut          _output1(p11);
+DigitalOut          _output2(p12);
+DigitalOut          _output3(p5);
+DigitalOut          _output4(p6);
 
 //Buzzer
-Beep _buzzer(p26);
+Beep                _buzzer(p26);
 
 //IMU
-FreeIMU _freeIMU;
+FreeIMU             _freeIMU;
+
+//Functions///////////////////////////////////////////////////////////////////////////////////////////////
+//Zero gyro, zero yaw and arm
+void Arm()
+{
+    //Zero gyro
+    _freeIMU.zeroGyro();
+    
+    //Set armed to true
+    _armed = true;   
+}
+
+//Set all RC to 0 and 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;   
+}
+
+//Zero pitch and roll
+void ZeroPitchRoll()
+{  
+    //Zero pitch and roll
+    float totalPitch = 0;
+    float totalRoll = 0;
+    float ypr[3] = {0,0,0}; // Yaw, pitch, roll
+    for(int i = 0; i < 500; i++)
+    {
+        _freeIMU.getYawPitchRoll(ypr);
+        totalPitch += ypr[1];
+        totalRoll += ypr[2];
+    }
+    
+    _zeroValues[1] = totalPitch/500;
+    _zeroValues[2] = totalRoll/500;
+    printf("Pitch %f\r\n", _zeroValues[1]);
+    printf("Roll %f\r\n", _zeroValues[2]);
+}
+
+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