Revision 0:c6a85bb2a827, committed 2015-03-04

Comitter:

joe4465

Date:

Wed Mar 04 18:50:37 2015 +0000

Child:

1:ec3521d90369

Commit message:

New version of quadcopter software, written following OO principles

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFileWrapper/ConfigFile.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFileWrapper/ConfigFileWrapper.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,161 @@
+#include "ConfigFileWrapper.h"
+
+ConfigFileWrapper::ConfigFileWrapper(){}
+
+ConfigFileWrapper::~ConfigFileWrapper(){}
+
+bool PidWrapper::initialise()
+{
+    _str = new char[1024];
+    loadSettings();
+    
+    DEBUG("Config file wrapper initialised");
+    return true; 
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getYawRateParameters()
+{
+    return _yawRateParameters;
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getPitchRateParameters()
+{
+    return _pitchRateParameters;
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getRollRateParameters()
+{
+    return _rollRateParameters;
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getYawStabParameters()
+{
+    return _yawStabParameters;
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getPitchStabParameters()
+{
+    return _pitchStabParameters;
+}
+
+pidWrapper::PidParameters ConfigFileWrapper::getRollStabParameters()
+{
+    return _rollStabParameters;
+}
+
+bool ConfigFileWrapper::setYawRateParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+bool ConfigFileWrapper::setPitchRateParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+bool ConfigFileWrapper::setRollRateParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+bool ConfigFileWrapper::setYawStabParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+bool ConfigFileWrapper::setPitchStabParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+bool ConfigFileWrapper::setRollStabParameters(pidWrapper::PidParameters)
+{
+    return true;
+}
+
+void ConfigFileWrapper::convertToCharArray(float number)
+{
+    sprintf(_str, "%1.8f", number );  
+}
+
+void ConfigFileWrapper::convertToCharArray(int number)
+{
+    sprintf(_str, "%d", number );  
+}
+
+void ConfigFileWrapper::loadSettings()
+{
+    char value[BUFSIZ];
+    
+    //Read a configuration file from a mbed.
+    if (!_configFile.read("/local/config.cfg"))
+    {
+        DEBUG("Config file does not exist\n\r");
+    }
+    else
+    {    
+        //Get values
+        if (_configFile.getValue("yawRatePIDControllerP", &value[0], sizeof(value))) _yawRateParameters.p = atof(value);
+        else DEBUG("Failed to get value for yawRatePIDControllerP\n\r");
+        
+        if (_configFile.getValue("yawRatePIDControllerI", &value[0], sizeof(value))) _yawRateParameters.i = atof(value);
+        else DEBUG("Failed to get value for yawRatePIDControllerI\n\r");
+
+        if (_configFile.getValue("yawRatePIDControllerD", &value[0], sizeof(value))) _yawRateParameters.d = atof(value);
+        else DEBUG("Failed to get value for yawRatePIDControllerD\n\r");
+
+        if (_configFile.getValue("pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRateParameters.p = atof(value);
+        else DEBUG("Failed to get value for pitchRatePIDControllerP\n\r");
+
+        if (_configFile.getValue("pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRateParameters.i = atof(value);
+        else DEBUG("Failed to get value for pitchRatePIDControllerI\n\r");
+
+        if (_configFile.getValue("pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRateParameters.d = atof(value);
+        else DEBUG("Failed to get value for pitchRatePIDControllerD\n\r");
+
+        if (_configFile.getValue("rollRatePIDControllerP", &value[0], sizeof(value))) _rollRateParameters.p = atof(value);
+        else DEBUG("Failed to get value for rollRatePIDControllerP\n\r");
+
+        if (_configFile.getValue("rollRatePIDControllerI", &value[0], sizeof(value))) _rollRateParameters.i = atof(value);
+        else DEBUG("Failed to get value for rollRatePIDControllerI\n\r");
+
+        if (_configFile.getValue("rollRatePIDControllerD", &value[0], sizeof(value))) _rollRateParameters.d = atof(value);
+        else DEBUG("Failed to get value for rollRatePIDControllerD\n\r");
+
+        
+        if (_configFile.getValue("yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabParameters.p = atof(value);
+        else DEBUG("Failed to get value for yawStabPIDControllerP\n\r");
+ 
+        if (_configFile.getValue("yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabParameters.i = atof(value);
+        else DEBUG("Failed to get value for yawStabPIDControllerI\n\r");
+
+        if (_configFile.getValue("yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabParameters.d = atof(value);
+        else DEBUG("Failed to get value for yawStabPIDControllerD\n\r");
+
+        if (_configFile.getValue("pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabParameters.p = atof(value);
+        else DEBUG("Failed to get value for pitchStabPIDControllerP\n\r");
+
+        if (_configFile.getValue("pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabParameters.i = atof(value);
+        else DEBUG("Failed to get value for pitchStabPIDControllerI\n\r");
+
+        if (_configFile.getValue("pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabParameters.d = atof(value);
+        else DEBUG("Failed to get value for pitchStabPIDControllerD\n\r");
+
+        if (_configFile.getValue("rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabParameters.p = atof(value);
+        else DEBUG("Failed to get value for rollStabPIDControllerP\n\r");
+
+        if (_configFile.getValue("rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabParameters.i = atof(value);
+        else DEBUG("Failed to get value for rollStabPIDControllerI\n\r");
+
+        if (_configFile.getValue("rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabParameters.d = atof(value);
+        else DEBUG("Failed to get value for rollStabPIDControllerD\n\r");
+
+        /*if (_configFile.getValue("zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
+        else DEBUG("Failed to get value for zero pitch\n\r");
+
+        if (_configFile.getValue("zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
+        else printf("Failed to get value for zero roll\n\r");*/
+    }
+    
+    DEBUG("Finished loading settings from config file\n\r");
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFileWrapper/ConfigFileWrapper.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,44 @@
+#include "mbed.h"
+#include "Global.h"
+#include "ConfigFile.h"
+#include "PidWrapper.h"
+
+#ifndef ConfigFileWrapper_H
+#define ConfigFileWrapper_H
+
+class ConfigFileWrapper                   // begin declaration of the class
+{
+  public:                    // begin public section
+    ConfigFileWrapper();    // constructor
+    ~ConfigFileWrapper();
+    
+    bool initialise();
+    pidWrapper::PidParameters getYawRateParameters();
+    pidWrapper::PidParameters getPitchRateParameters();
+    pidWrapper::PidParameters getRollRateParameters();
+    pidWrapper::PidParameters getYawStabParameters();
+    pidWrapper::PidParameters getPitchStabParameters();
+    pidWrapper::PidParameters getRollStabParameters();
+    
+    bool setYawRateParameters(pidWrapper::PidParameters);
+    bool setPitchRateParameters(pidWrapper::PidParameters);
+    bool setRollRateParameters(pidWrapper::PidParameters);
+    bool setYawStabParameters(pidWrapper::PidParameters);
+    bool setPitchStabParameters(pidWrapper::PidParameters);
+    bool setRollStabParameters(pidWrapper::PidParameters);
+    
+  private:
+    ConfigFile _configFile;
+    char* _str
+    pidWrapper::PidParameters _yawRateParameters;
+    pidWrapper::PidParameters _pitchRateParameters;
+    pidWrapper::PidParameters _rollRateParameters;
+    pidWrapper::PidParameters _yawStabParameters;
+    pidWrapper::PidParameters _pitchStabParameters;
+    pidWrapper::PidParameters _rollStabParameters;
+    void convertToCharArray(float number);
+    void convertToCharArray(int number);
+    void loadSettings();
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/FlightController.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,62 @@
+#include "FlightController.h"
+
+FlightController::FlightController(){}
+
+FlightController::~FlightController(){}
+
+bool FlightController::initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+{
+    _status = status;
+    
+    _motorMixer.initialise(motor1, motor2, motor3, motor4);
+    _rateController.initialise(sensors, navigationController);
+    _stabController.initialise(sensors, navigationController);
+   
+    _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void *)0);
+    int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
+    _rtosTimer->start(updateTime);
+    DEBUG("Flight controller initialised");
+    return true; 
+}
+
+void FlightController::threadStarter(void const *p)
+{
+    FlightController *instance = (FlightController*)p;
+    instance->threadWorker();
+}
+
+void FlightController::threadWorker()
+{
+    //Struct to hold PID outputs
+    PidWrapper::PidOutputs pidOutputs;
+    
+    //Check state is valid
+    Status::State state = _status.getState();
+    if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO)  
+    {
+        //State valid
+        Status::FlightMode flightMode = _status.getFlightMode();
+        if(flightMode == Status::RATE)
+        {
+            //Rate mode  
+            pidOutputs = _rateController.compute(); 
+        }
+        else if (flightMode == Status::STAB)
+        {
+            //Stab mode
+            pidOutputs = _stabController.compute();    
+        }
+        else
+        {
+            //Invalid state
+            _status.setState(Status::ABORT);
+            return;
+        }  
+        
+        _motorMixer.computePower(pidOutputs, throttle);       
+    }
+    //Set motors to armed if state is ground ready
+    else if (state == Status::GROUND_READY) _motorMixer.setPower(MOTORS_ARMED);
+    //Disable motors if state is not valid
+    else _motorMixer.setPower(MOTORS_OFF);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/FlightController.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "Global.h"
+#include "rtos.h"
+#include "Status.h"
+#include "MotorMixer.h"
+#include "PidWrapper.h"
+#include "RateController.h"
+#include "StabController.h"
+
+#ifndef FlightController_H
+#define FlightController_H
+
+class FlightController               
+{
+  public:                  
+    FlightController(); 
+    ~FlightController();
+    
+    bool initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4);
+    PidWrapper::PidValues getyawRatePID();
+    PidWrapper::PidValues getPitchRatePID();
+    PidWrapper::PidValues getRollRatePID();
+    PidWrapper::PidValues getyawStabPID();
+    PidWrapper::PidValues getPitchStabPID();
+    PidWrapper::PidValues getRollStabPID();
+    MotorMixer::MotorPower getMotorPower();
+    
+  private:
+    static void threadStarter(void const *p);
+    void threadWorker();
+    RtosTimer* _rtosTimer;
+    Status _status;
+    MotorMixer _motorMixer;
+    RateController _rateController;
+    StabController _stabController;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/MotorMixer/MotorMixer.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,84 @@
+#include "MotorMixer.h"
+
+MotorMixer::MotorMixer(){}
+
+MotorMixer::~MotorMixer(){}
+
+bool MotorMixer::initialise(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+{
+    _motor1 = new PwmOut(motor1);
+    _motor2 = new PwmOut(motor2);
+    _motor3 = new PwmOut(motor3);
+    _motor4 = new PwmOut(motor4);
+    
+    //Set frequency
+    float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
+    _motor1->period(period);
+    _motor2->period(period);
+    _motor3->period(period);
+    _motor4->period(period);
+    
+    //Disable
+    _motor1 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    _motor2 = MOTORS_OFF;
+    
+    DEBUG("Motor power initialised");
+    return true; 
+}
+
+void MotorMixer::computePower(PidWrapper::PidOutputs pidOutputs, float throttle)
+{
+    //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
+    float basePower = MOTORS_MIN + (throttle * 800);
+    
+    //Map motor power - each PID returns -100 <-> 100
+    _motorPower.motor1 = basePower + pidOutputs.pitch + pidOutputs.roll + pidOutputs.yaw;
+    _motorPower.motor2 = basePower + pidOutputs.pitch - pidOutputs.roll - pidOutputs.yaw;
+    _motorPower.motor3 = basePower - pidOutputs.pitch - pidOutputs.roll + pidOutputs.yaw;
+    _motorPower.motor4 = basePower - pidOutputs.pitch + pidOutputs.roll - pidOutputs.yaw;
+    
+    //Specify intial motor power limits
+    float motorFix = 0;
+    float motorMin = _motorPower.motor1;
+    float motorMax = _motorPower.motor1;
+    
+    //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
+    if(_motorPower.motor1 < motorMin) motorMin = _motorPower.motor1;
+    if(_motorPower.motor1 > motorMax) motorMax = _motorPower.motor1;
+    if(_motorPower.motor2 < motorMin) motorMin = _motorPower.motor2;
+    if(_motorPower.motor2 > motorMax) motorMax = _motorPower.motor2;
+    if(_motorPower.motor3 < motorMin) motorMin = _motorPower.motor3;
+    if(_motorPower.motor3 > motorMax) motorMax = _motorPower.motor3;
+    if(_motorPower.motor4 < motorMin) motorMin = _motorPower.motor4;
+    if(_motorPower.motor4 > motorMax) motorMax = _motorPower.motor4;
+        
+    //Check if min or max is outside of the limits
+    if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
+    else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
+    
+    //Add/remove constant
+    _motorPower.motor1 += motorFix;
+    _motorPower.motor2 += motorFix;
+    _motorPower.motor3 += motorFix;
+    _motorPower.motor4 += motorFix;
+}
+
+void MotorMixer::setPower(float motor1Power, float motor2Power, float motor3Power, float motor4Power)
+{
+    _motor1->pulsewidth_us(motor1Power);
+    _motor2->pulsewidth_us(motor2Power);
+    _motor3->pulsewidth_us(motor3Power);
+    _motor4->pulsewidth_us(motor4Power);
+}
+
+void MotorMixer::setPower(float motorPower)
+{
+    setPower(motorPower, motorPower, motorPower, motorPower);
+}
+
+MotorMixer::MotorPower MotorMixer::getMotorPower()
+{
+    return _motorPower;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/MotorMixer/MotorMixer.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,37 @@
+#include "mbed.h"
+#include "Global.h"
+#include "PidWrapper.h"
+
+#ifndef MotorMixer_H
+#define MotorMixer_H
+
+class MotorMixer
+{
+  public:                 
+    MotorMixer();   
+    ~MotorMixer();
+    
+    struct MotorPower
+    {
+       float motor1;
+       float motor2;
+       float motor3;
+       float motor4;
+    };
+    
+    bool initialise(PinName motor1, PinName motor2, PinName motor3, PinName motor4);
+    void computePower(PidWrapper::PidOutputs pidOutputs, float throttle);
+    void setPower(float motorPower);
+    void setPower(float motor1Power, float motor2Power, float motor3Power, float motor4Power);
+    MotorPower getMotorPower();
+    
+  private:
+    PwmOut*  _motor1;
+    PwmOut*  _motor2;
+    PwmOut*  _motor3;
+    PwmOut*  _motor4;
+    
+    MotorPower _motorPower; 
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/RateController/RateController.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,39 @@
+#include "RateController.h"
+
+RateController::RateController(){}
+
+RateController::~RateController(){}
+
+bool RateController::initialise(Sensors& sensors, NavigationController& navigationController)
+{
+    _sensors = sensors;
+    _navigationController = navigationController;
+    
+    ConfigFileWrapper configFileWrapper = new ConfigFileWrapper();
+    configFileWrapper.initialise();
+    
+    _yawRatePidController.initialise(configFileWrapper.getYawRateParameters, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
+    _pitchRatePidController.initialise(configFileWrapper.getPitchRateParameters, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
+    _rollRatePidController.initialise(configFileWrapper.getRollRateParameters, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
+
+    DEBUG("Rate controller initialised");
+    return true; 
+}
+
+PidWrapper::PidOutputs RateController::compute()
+{
+    //Update rate PID process value with gyro rate
+    _yawRatePIDController->setProcessValue(_gyroRate[0]);
+    _pitchRatePIDController->setProcessValue(_gyroRate[1]);
+    _rollRatePIDController->setProcessValue(_gyroRate[2]);
+    
+    //Update rate PID set point with desired rate from RC
+    _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
+    _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
+    _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
+    
+    //Compute rate PID outputs
+    _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
+    _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
+    _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FlightController/RateController/RateController.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,26 @@
+#include "mbed.h"
+#include "Global.h"
+#include "PidWrapper.h"
+#include "ConfigFileWrapper.h"
+
+#ifndef RateController_H
+#define RateController_H
+
+class RateController               
+{
+  public:                  
+    RateController(); 
+    ~RateController();
+    
+    bool initialise(Sensors& sensors, NavigationController& navigationController);
+    PidWrapper::PidOutputs compute();
+    
+  private:
+    Sensors _sensors;
+    NavigationController _navigationController;
+    PidWrapper _yawRatePIDController;
+    PidWrapper _pitchRatePIDController;
+    PidWrapper _rollRatePIDController;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Global.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+
+#ifndef GLOBAL_H
+#define GLOBAL_H
+
+#if 1
+  #define DEBUG(a) printf(a)
+#else
+  #define DEBUG(a) (void)0
+#endif
+
+#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 90
+#define             RC_ROLL_RATE_MIN -90
+#define             RC_PITCH_RATE_MAX 90
+#define             RC_PITCH_RATE_MIN -90
+#define             RC_ROLL_ANGLE_MAX 20
+#define             RC_ROLL_ANGLE_MIN -20
+#define             RC_PITCH_ANGLE_MAX 20
+#define             RC_PITCH_ANGLE_MIN -20
+#define             RC_THRUST_MAX 1
+#define             RC_THRUST_MIN 0
+#define             RC_VELOCITY_MAX 2.5
+#define             RC_VELOCITY_MIN 2.5
+
+#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
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidWrapper/PID.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidWrapper/PidWrapper.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,24 @@
+#include "PidWrapper.h"
+
+PidWrapper::PidWrapper(){}
+
+PidWrapper::~PidWrapper(){}
+
+bool PidWrapper::initialise(PidParameters pidParameters, float inputMin, float inputMax, float outputMin, float outputMax)
+{
+    float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
+    _pid = new PID(pidParameters.p, pidParameters.i, pidParameters.d, updateTime);
+    _pid->setInputLimits(inputMin, inputMax);
+    _pid->setOutoutLimits(outputMin, outputMax);
+    _pid->setMode(AUTO_MODE);
+    _pid->setSetPoint(0.0);
+    _pid->setBias(0);
+    
+    DEBUG("PID wrapper initialised");
+    return true; 
+}
+
+PidWrapper::PidValues pidWrapper::compute(PidValues pidValues)
+{
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidWrapper/PidWrapper.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,41 @@
+#include "mbed.h"
+#include "Global.h"
+
+#ifndef PidWrapper_H
+#define PidWrapper_H
+
+class PidWrapper                   // begin declaration of the class
+{
+  public:                    // begin public section
+    PidWrapper();    // constructor
+    ~PidWrapper();
+    
+    struct PidOutputs
+    {
+        float yaw;
+        float pitch;
+        float roll;
+    };
+    
+    struct PidParameters
+    {
+        float p;
+        float i;
+        float d;
+    };
+    
+    struct PidValues
+    {
+        float setPoint;
+        float processValue;
+        float output;    
+    };
+    
+    bool initialise(PidParameters pidParameters, float inputMin, float inputMax, float outputMin, float outputMax);
+    PidValues compute(PidValues pidValues);
+    
+  private:
+    PID* _pid;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rc/Ppm.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/joe4465/code/PPM/#b67f18c84c05

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rc/Rc.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,111 @@
+#include "Rc.h"
+
+// A class to get RC commands and convert to correct values
+//Channel 0 is roll. min 1000. max 1900
+//Channel 1 is pitch. min 1000. max 1900
+//Channel 2 is throttle < 900 when not connected. min 1000. max 1900
+//Channel 3 is yaw. min 1000. max 1900
+//Channel 4 is arm. armed > 1800 else unarmed
+//Channel 5 is mode. rate > 1800. stab < 1100
+//Channel 6 is spare
+//Channel 7 is spare
+
+Rc::Rc(){}
+
+Rc::~Rc(){}
+
+bool Rc::initialise(Status& status, PinName pin)
+{
+    _status = status;
+    
+    _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
+    _yawMedianFilter = new medianFilter(5);
+    _pitchMedianFilter = new medianFilter(5);
+    _rollMedianFilter = new medianFilter(5);
+    _thrustMedianFilter = new medianFilter(5);
+    
+    _thread = new Thread(&Rc::threadStarter, this, osPriorityHigh);
+    DEBUG("Rc initialised");
+    return true; 
+}
+
+void Rc::threadStarter(void const *p)
+{
+    Rc *instance = (Rc*)p;
+    instance->threadWorker();
+}
+
+void Rc::threadWorker()
+{
+    while(true)
+    {
+        float rc[8] = {0,0,0,0,0,0,0,0};
+        
+        //Get channel data - mapped to between 0 and 1
+        _ppm->GetChannelData(rc);
+        
+        //Put channel data into raw rc struct
+        _rawRc.channel0 = rc[0];
+        _rawRc.channel1 = rc[1];
+        _rawRc.channel2 = rc[2];
+        _rawRc.channel3 = rc[3];
+        _rawRc.channel4 = rc[4];
+        _rawRc.channel5 = rc[5];
+        _rawRc.channel6 = rc[6];
+        _rawRc.channel7 = rc[7];
+        
+        //Check whether transmitter is connected
+        if(_rawRc.channel2 != -1)
+        {
+            _status.setRcConnected(true);
+            
+            //Map yaw channel
+            _mappedRc.yaw = - _yawMedianFilter->process(Map(_rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
+        
+            //Map thust channel
+            _mappedRc.throttle = _thrustMedianFilter->process(Map(_rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
+        
+            //Map arm channel.
+            if(_rawRc.channel4 > 0.5) _status.setArmed(true);
+            else _status.setArmed(false);
+            
+            //Map mode channel
+            if(_rawRc.channel5 < 0.5) _status.setFlightMode(Status::STAB);
+            else _status.setFlightMode(Status::RATE);
+        
+            //Roll and pitch mapping depends on the mode
+            if(_status.getFlightMode() == Status::STAB)//Stability mode
+            {
+                //Roll
+                _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
+                //Pitch
+                _mappedRc.pitch = _pitchMedianFilter->process(-Map(_rawRc.channel1, RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX)); //Needs to be reverse
+            }
+            else if(_status.getFlightMode() == Status::RATE)//Rate mode
+            {
+                //Roll
+                _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
+                //Pitch
+                _mappedRc.pitch = _pitchMedianFilter->process(-Map(_rawRc.channel1, RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX)); //Needs to be reverse
+            }
+        }
+        else _status.setRcConnected(false);
+        
+        Thread::wait(20);
+    }
+}
+
+float Rc::Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
+{
+    return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
+}
+
+Rc::MappedRc Rc::getMappedRc()
+{
+    return _mappedRc;
+}
+
+Rc::RawRc Rc::getRawRc()
+{
+    return _rawRc;  
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rc/Rc.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,57 @@
+#include "mbed.h"
+#include "Global.h"
+#include "Ppm.h"
+#include "filter.h"
+#include "Status.h"
+#include "rtos.h"
+
+#ifndef Rc_H
+#define Rc_H
+
+class Rc                
+{
+  public:             
+    Rc();    
+    ~Rc();
+    
+    struct MappedRc
+    {
+       float roll;
+       float pitch;
+       float throttle;
+       float yaw;
+    };
+    
+    struct RawRc
+    {
+        int channel0;
+        int channel1;
+        int channel2;
+        int channel3;
+        int channel4;
+        int channel5;
+        int channel6;
+        int channel7;    
+    };
+    
+    bool initialise(Status& status, PinName pin);
+    MappedRc getMappedRc();
+    RawRc getRawRc();
+    
+  private:
+    static void threadStarter(void const *p);
+    void threadWorker();
+    float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
+    
+    Ppm* _ppm;
+    Thread* _thread;
+    Status _status;
+    MappedRc _mappedRc;
+    RawRc _rawRc;
+    medianFilter* _yawMedianFilter;
+    medianFilter* _pitchMedianFilter;
+    medianFilter* _rollMedianFilter;
+    medianFilter* _thrustMedianFilter;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rc/filter.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/networker/code/filter/#46a72e790df8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Imu/FreeIMU_external_magnetometer.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tyftyftyf/code/FreeIMU/#48a0eae27bf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/LidarLite.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/akashvibhute/code/LidarLite/#8e6304ab38d2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/MaxbotixDriver.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/joe4465/code/MaxbotixDriver/#24d9d6d213aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Status/Status.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,145 @@
+#include "Status.h"
+
+Status::Status(){}
+
+Status::~Status(){}
+
+bool Status::initialise()
+{
+    setState(PREFLIGHT);
+}
+
+bool Status::setState(State state)
+{
+    switch(state)
+    {
+        case PREFLIGHT:
+            setFlightMode(NOT_SET);
+            setBaseStationMode(STATUS);
+            setBatteryLevel(0);
+            setArmed(false);
+            setInitialised(false);
+            return true;
+              
+        case STANDBY:
+        
+            return true;
+              
+            
+        case GROUND_READY:
+        
+            return true;
+              
+            
+        case MANUAL:
+        
+            return true;
+              
+        
+        case STABILISED:
+            
+            return true;
+              
+        
+        case AUTO:
+            
+            return true;
+              
+        
+        case ABORT:
+            
+            return true;
+              
+            
+        case EMG_LAND:
+            
+            return true;
+              
+            
+        case EMG_OFF:
+            
+            return true;
+              
+            
+        case GROUND_ERROR:
+            
+            return true;
+              
+        
+        default:
+            
+            return false;
+              
+    }    
+}
+
+Status::State Status::getState()
+{
+    return _state;    
+}
+
+bool Status::setFlightMode(FlightMode flightMode)
+{
+    _flightMode = flightMode;
+    return true;
+}
+
+Status::FlightMode Status::getFlightMode()
+{
+    return _flightMode;
+}
+
+bool Status::setBaseStationMode(BaseStationMode baseStationMode)
+{
+    _baseStationMode = baseStationMode;
+    return true;
+}
+
+Status::BaseStationMode Status::getBaseStationMode()
+{
+    return _baseStationMode;
+}
+
+bool Status::setBatteryLevel(float batteryLevel)
+{
+    _batteryLevel = batteryLevel;
+    return true;
+}
+
+float Status::getBatteryLevel()
+{
+    return _batteryLevel;
+}
+
+bool Status::setArmed(bool armed)
+{
+    _armed = armed;
+    return true;
+}
+
+bool Status::getArmed()
+{
+    return _armed;
+}
+
+bool Status::setInitialised(bool initialised)
+{
+    _initialised = initialised;
+    return true;
+}
+
+bool Status::getInitialised()
+{
+    return _initialised;
+}
+
+bool Status::setRcConnected(bool rcConnected)
+{
+    _rcConnected = rcConnected;
+    return true;
+}
+
+bool Status::getRcConnected()
+{
+    return _rcConnected;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Status/Status.h	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+
+#ifndef Status_H
+#define Status_H
+
+class Status                   // begin declaration of the class
+{
+  public:                    // begin public section
+    Status();     // constructor
+    ~Status();                  // destructor
+    
+    enum State
+    {
+        PREFLIGHT,
+        STANDBY,
+        GROUND_READY,
+        MANUAL,
+        STABILISED,
+        AUTO,
+        ABORT,
+        EMG_LAND,
+        EMG_OFF,
+        GROUND_ERROR     
+    };
+    
+    enum FlightMode
+    {
+        RATE,
+        STAB,
+        NOT_SET  
+    };
+    
+    enum BaseStationMode
+    {
+        MOTOR_POWER,
+        PID_OUTPUTS,
+        IMU_OUTPUTS,
+        STATUS,
+        MAPPED_RC,
+        PID_TUNING,
+        GPS,
+        ZERO,
+        RATE_TUNING,
+        STAB_TUNING,
+        ALTITUDE,
+        VELOCITY   
+    };
+    
+    bool initialise();
+    bool setState(State state);
+    State getState();
+    bool setFlightMode(FlightMode flightMode);
+    FlightMode getFlightMode();
+    bool setBaseStationMode(BaseStationMode baseStationMode);
+    BaseStationMode getBaseStationMode();
+    bool setBatteryLevel(float batteryLevel);
+    float getBatteryLevel();
+    bool setArmed(bool armed);
+    bool getArmed();
+    bool setInitialised(bool initialised);
+    bool getInitialised();
+    bool setRcConnected(bool rcConnected);
+    bool getRcConnected();
+    
+  private:             
+    State _state; 
+    FlightMode _flightMode;             
+    BaseStationMode _baseStationMode;
+    float _batteryLevel;
+    bool _armed;
+    bool _initialised;
+    bool _rcConnected;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "Global.h"
+#include "MODSERIAL.h"
+#include "Status.h"
+#include "Sensors.h"
+#include "BaseStation.h"
+#include "Rc.h"
+#include "FlightController.h"
+#include "NavigationController.h"
+
+MODSERIAL                           _debug(USBTX, USBRX);
+
+//Wireless Serial
+MODSERIAL                           _base(p9, p10);
+
+//GPS Serial
+//MODSERIAL                           _gps(p13, p14);
+
+//Onboard LED's
+//DigitalOut                          _led1(LED1);
+//DigitalOut                          _led2(LED2);
+//DigitalOut                          _led3(LED3);
+//DigitalOut                          _led4(LED4);
+
+//MaxBotix Ping sensor
+//Timer                               _maxBotixTimer;
+//Sonar                               _maxBotixSensor(p15, _maxBotixTimer);
+
+//Lidar
+//LidarLite                           _lidar(p28, p27);
+
+//Unused analog pins
+DigitalOut                          _spare1(p16);
+DigitalOut                          _spare2(p17);
+DigitalOut                          _spare3(p18);
+DigitalOut                          _spare4(p19);
+
+//Classes
+Status                              _status;
+//Sensors                             *_sensors(_status, p13, p14, p15, p28, p27);
+//BaseStation                         *_baseStation(p9, p10);
+Rc                                  _rc;
+FlightController                    _flightController;
+//NavigationController                *_navigationController;           
+
+int main()
+{
+    _base.baud(115200);
+    
+    DEBUG("\r\n");  
+    DEBUG("********************************************************************************\r\n");
+    DEBUG("Starting Setup\r\n");
+    DEBUG("********************************************************************************\r\n");
+    
+    //Set Status
+    _status.initialise();
+    
+    //Initalise Base Station
+        
+    //Initialise RC
+    _rc.initialise(_status, p8);
+    
+    //Initialise Sensors
+    
+    //Initialise Navigation
+    
+    //Initialise Flight Controller
+    _flightController.initialise(_status, _sensors, _navigationController, p21, p22, p23, p24);
+    
+    
+    
+    DEBUG("********************************************************************************\r\n");
+    DEBUG("Finished Setup\r\n");
+    DEBUG("********************************************************************************\r\n"); 
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#721c3a93a7b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 04 18:50:37 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac
\ No newline at end of file