File content as of revision 3:82665e39f1ea:

#include "mbed.h"
#include "rtos.h"
#include "FreeIMU.h"
#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_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
//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;

//Threads
Thread              *_statusThread;
Thread              *_serialPortMonitorThread;
Thread              *_flightControllerThread;
Thread              *_rcCommandMonitorThread;

//Config file
LocalFileSystem     local("local");
ConfigFile          _configFile;

//HARDWARE////////////////////////////////////////////////////////////////////////////////////
// M1  M2
//  \  /
//   \/
//   /\
//  /  \
// M3  M4
 
//Motors
PwmOut              _motor1(p22);
PwmOut              _motor2(p23);
PwmOut              _motor3(p24);
PwmOut              _motor4(p25);

//USB serial
Serial              _wiredSerial(USBTX, USBRX);

//Wireless Serial
Serial              _wirelessSerial(p9, p10);

//GPS Serial
//Serial              _gpsSerial(p28, p27);

//PPM in
PPM *_ppm;
InterruptIn *_interruptPin = new InterruptIn(p5);

//Battery monitor
//DigitalIn           _batteryMonitor(p8);

//Onboard LED's
DigitalOut          _led1(LED1);
DigitalOut          _led2(LED2);
DigitalOut          _led3(LED3);
DigitalOut          _led4(LED4);

//Outputs
DigitalOut          _output1(p11);
DigitalOut          _output2(p12);
DigitalOut          _output3(p5);
DigitalOut          _output4(p6);

//Buzzer
Beep                _buzzer(p26);

//IMU
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