File content as of revision 3:82665e39f1ea:

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

//Declarations
void RcCommandMonitorTask(void const *n);

//Timers
RtosTimer       *_rcCommandMonitorUpdateTimer;

// A thread to control flight
void RcCommandMonitorThread(void const *args) 
{         
    //Update Timer
    _rcCommandMonitorUpdateTimer = new RtosTimer(RcCommandMonitorTask, osTimerPeriodic, (void *)0);
    int updateTime = (1.0 / RC_COMMANDS_FREQUENCY) * 1000;
    _rcCommandMonitorUpdateTimer->start(updateTime);
    
    // Wait here forever
    Thread::wait(osWaitForever);
}

//Get RC commands
//Channel 1 is roll. min 1000. max 1900
//Channel 2 is pitch. min 1000. max 1900
//Channel 3 is throttle < 900 when not connected. min 1000. max 1900
//Channel 4 is yaw. min 1000. max 1900
//Channel 5 is arm. armed > 1800 else unarmed
//Channel 6 is mode. rate > 1800. stab < 1100
//Channel 7 is spare
//Channel 8 is spare
void RcCommandMonitorTask(void const *n)
{
    //Initialise array to hold channel data
    float rcCommands[8] = {0,0,0,0,0,0,0,0};
    
    //Get channel data - mapped to between 0 and 1
    _ppm->GetChannelData(rcCommands);
    
    //Map yaw channel
    _rcMappedCommands[0] = Map(rcCommands[3], 0, 1, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
    
    //Map thust channel
    _rcMappedCommands[3] = Map(rcCommands[2], 0, 1, RC_THRUST_MIN, RC_THRUST_MAX);
    
    //Map arm channel. 0.85 = 1765
    if(rcCommands[4] > 0.85) Arm();
    else Disarm();
    
    //Map mode channel
    if(rcCommands[5] < 0.5)
    {
        _stab = true;
        _rate = false;
    }
    else
    {
        _stab = false;
        _rate = true;
    }  
    
    //Roll and pitch mapping depends on the mode
    if(_rate == false && _stab == true)//Stability mode
    {
        //Roll
        _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
        //Pitch
        _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
    }
    else if(_rate == true && _stab == false)//Rate mode
    {
        //Roll
        _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
        //Pitch
        _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
    }
    else
    {
        _rcMappedCommands[1] = 0;
        _rcMappedCommands[2] = 0;
    }
}