File content as of revision 1:045edcf091f3:

#include "mbed.h"
#include "rtos.h"
#include "hardware.h"

//Declarations
float Constrain(const float in, const float min, const float max);
float map(float x, float in_min, float in_max, float out_min, float out_max);
void GetAttitude();
void Task500Hz(void const *n);
void Task10Hz();

//Variables
float _gyroRate[3] ={}; // Yaw, Pitch, Roll
float _yawTarget = 0;
int _notFlying = 0; 
float _altitude = 0;
int _10HzIterator = 0;

//Timers
RtosTimer *_updateTimer;

// A thread to monitor the serial ports
void FlightControllerThread(void const *args) 
{  
    //Update Timer
    _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
    int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
    _updateTimer->start(updateTime);
    
    // Wait here forever
    Thread::wait(osWaitForever);
}

//Constrains value to between min and max
float Constrain(const float in, const float min, const float max)
{
    float out = in;
    out = out > max ? max : out;
    out = out < min ? min : out;
    return out;
}

//Maps input to output
float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//Zeros values
void GetAttitude()
{
    //Take off zero values to account for any angle inbetween the PCB level and ground
    _ypr[1] = _ypr[1] - _zeroValues[1];
    _ypr[2] = _ypr[2] - _zeroValues[2];
    
    //Swap pitch and roll because IMU is mounted at a right angle to the board
    float pitch = _ypr[2];
    float roll = _ypr[1];
    _ypr[1] = pitch;
    _ypr[2] = roll;
}

void Task500Hz(void const *n)
{
    _10HzIterator++;
    if(_10HzIterator % 50 == 0)
    {
        Task10Hz();
    }

    //Get IMU data
    _freeIMU.getYawPitchRoll(_ypr);
    _freeIMU.getRate(_gyroRate);
    GetAttitude();
    
    //Rate mode
    if(_rate == true && _stab == false)
    {
        //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();
    }
    //Stability mode
    else
    {
        //Update stab PID process value with ypr
        _yawStabPIDController->setProcessValue(_ypr[0]);
        _pitchStabPIDController->setProcessValue(_ypr[1]);
        _rollStabPIDController->setProcessValue(_ypr[2]);
        
        //Update stab PID set point with desired angle from RC
        _yawStabPIDController->setSetPoint(_yawTarget);
        _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
        _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
        
        //Compute stab PID outputs
        _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
        _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
        _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
        
        //If pilot commanding yaw
        if(abs(_rcMappedCommands[0]) > 0)
        {  
            _stabPIDControllerOutputs[0] = _rcMappedCommands[0];  //Feed to rate PID (overwriting stab PID output)
            _yawTarget = _ypr[0];
        }
        
        //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 stab PID
        _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
        _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
        _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
        
        //Compute rate PID outputs
        _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
        _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
        _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
    }
    
    //Testing
    _ratePIDControllerOutputs[0] = 0; // yaw
    //_ratePIDControllerOutputs[1] = 0; // pitch
    _ratePIDControllerOutputs[2] = 0; // roll

    //Calculate motor power if flying
    if(_rcMappedCommands[3] > 0.1 && _armed == true)
    {
        //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
        _motorPower[0] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[1] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[2] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[3] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
    
        
        //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1500); //Reduced to 1500 to limit power for testing
        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1500);
        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1500);
        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1500);
    }

    //Not flying
    else if(_armed == true)
    {
        _yawTarget = _ypr[0];
        
        //Set motors to armed state
        _motorPower[0] = MOTORS_ARMED;
        _motorPower[1] = MOTORS_ARMED;
        _motorPower[2] = MOTORS_ARMED;
        _motorPower[3] = MOTORS_ARMED;
        
        _notFlying ++;
        if(_notFlying > 500) //Not flying for 1 second
        {
            //Reset iteratior
            _notFlying = 0;
            
            //Reset I
            _yawRatePIDController->reset();
            _pitchRatePIDController->reset();
            _rollRatePIDController->reset();
            _yawStabPIDController->reset();
            _pitchStabPIDController->reset();
            _rollStabPIDController->reset();
        }
    } 
    else
    {
        //Disable Motors
        _motorPower[0] = MOTORS_OFF;
        _motorPower[1] = MOTORS_OFF;
        _motorPower[2] = MOTORS_OFF;
        _motorPower[3] = MOTORS_OFF;
    }
    
    //Set motor power
    _motor1.pulsewidth_us(_motorPower[0]);
    _motor2.pulsewidth_us(_motorPower[1]);
    _motor3.pulsewidth_us(_motorPower[2]);
    _motor4.pulsewidth_us(_motorPower[3]);
}

//Print data
void Task10Hz()
{
    int batt = 0;
    _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f>",
    _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
}