File content as of revision 0:c6a85bb2a827:

#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);
}