File content as of revision 0:c6a85bb2a827:

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