File content as of revision 4:dc844fde64d7:

#include "RCin.h"


using namespace std;

RCin::RCin(PinName pin) : pwm1(pin) , dout3(PA_10)
{
    pwm1.fall(callback(this, &RCin::fall_edge));
    pwm1.rise(callback(this, &RCin::rise_edge));
    local_ti.start();
    local_ti.reset();
    for(uint8_t k = 0;k<20;k++)
        pwms[k] = 0;
    isAlive = false;
    }

// *****************************************************************************
void RCin::map_Channels(void){
    CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0);
    CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0);
    CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0);
    CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
    }

// *****************************************************************************
void RCin::rise_edge(void)
    {
     local_ti.reset();
     }

// *****************************************************************************
void RCin::fall_edge(void)
   {
        uint32_t time_us = local_ti.read_us();
        if(time_us > 2500){
            chnr = 0;
            map_pwm_2_PM1();
        }
        else
            pwms[++chnr] = time_us;
    }
// *****************************************************************************
uint8_t RCin::get_flightmode(void){
    if(isAlive){
        return current_flightmode;
        }
    else 
        return 0;
    }
// *****************************************************************************
void RCin::map_pwm_2_PM1(void){
//    data.RC_PM1[0]=CH1_2_PM1((float)pwms[1]);       // CH1 -> Roll
//    data.RC_PM1[1]=CH2_2_PM1((float)pwms[2]);       // CH2 -> Pitch
//    data.RC_PM1[3]=CH3_2_PM1((float)pwms[3]);       // CHANGED HERE: CH3 -> Thrust!!!
//    data.RC_PM1[2]=CH4_2_PM1((float)pwms[4]);       // CHANGED HERE: CH4 -> Yaw!!!


    dout3.write(1);

    PM1[0]=CH1_2_PM1((float)pwms[1]);       // CH1 -> Roll
    PM1[1]=CH2_2_PM1((float)pwms[2]);       // CH2 -> Pitch
    PM1[3]=CH3_2_PM1((float)pwms[3]);       // CHANGED HERE: CH3 -> Thrust!!!
    PM1[2]=CH4_2_PM1((float)pwms[4]);       // CHANGED HERE: CH4 -> Yaw!!!
                                        // RC Chan     5    6
    if(pwms[5] >620 && pwms[5] < 720)               // 2    1 
        current_flightmode = LOITER;                
    else if(pwms[5] >720 && pwms[5] < 850)          // 2    2
        current_flightmode = AUTO;
    else if(pwms[5] >870 && pwms[5] < 970)          // 2    3
        current_flightmode = RTL;
    else if(pwms[5] >1000 && pwms[5] < 1100)        // 1    1
        current_flightmode = STABILIZED;
    else if(pwms[5] >1100 && pwms[5] < 1200)        // 1    2
        current_flightmode = ALT_HOLD;
    else if(pwms[5] >1200 && pwms[5] < 1350)        // 1    3
        current_flightmode = VEL_OF_Z_POS;
    else
        current_flightmode = STABILIZED;
    if(pwms[3]>550)
        isAlive = true;
    if(old_flightmode != current_flightmode)
        flightmode_changed = true;
    old_flightmode = current_flightmode;

dout3.write(0);    
    }