File content as of revision 6:4c207e7b1203:

#include "mbed.h"
#include "rtos.h"
#include "FreeIMU.h"
#include "PID.h"
#include "ConfigFile.h"
#include "PPM.h"
#include "filter.h"
#include <sstream>

#ifndef HARDWARE_H
#define HARDWARE_H

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

#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

//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);

//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
bool                _armed = false;
bool                _rate = false;
bool                _stab = true;
bool                _initialised = false;
bool                _gpsConnected = false;
float               _motorPower [4] = {0,0,0,0};
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;
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;
char*               _str = new char[1024];

//RC filters
medianFilter        *_yawMedianFilter;
medianFilter        *_pitchMedianFilter;
medianFilter        *_rollMedianFilter;
medianFilter        *_thrustMedianFilter;
medianFilter        *_channel7MedianFilter;
medianFilter        *_channel8MedianFilter;

//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(p7);

//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);

//IMU
FreeIMU             _freeIMU;

//Functions///////////////////////////////////////////////////////////////////////////////////////////////
//Zero gyro and arm
void Arm()
{
    //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; 
    }  
}

//Disarm
void Disarm()
{   
    //Set armed to false
    _armed = false;  
    
    //Disable modes
    _levelOffset = false; 
    
    //Save settings
    WriteSettingsToConfig();
}

//Zero pitch and roll
/*void ZeroPitchRoll()
{  
    printf("Zeroing pitch and roll\r\n");
    
    //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]);
}  */  

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