File content as of revision 7:70161eb0f854:

#include "mbed.h"
#include "SBUS.hpp"
#include "LoopTicker.hpp"


SBUS sbus(PD_5, PD_6);
Serial pc(USBTX, USBRX);

DigitalOut led1(LED1);
DigitalOut led3(LED3);
PwmOut servo1(PB_4);
PwmOut servo2(PB_5);

int ch1, ch2;
float rc1, rc2;
int out1, out2;

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void interrupt_SBUS()
{
    led1 = !led1;
    if(sbus.failSafe == false)
    {
        ch1 = int(sbus.getData(1));
        ch2 = int(sbus.getData(2));
        pc.printf("ch1 :%d ", ch1);
        pc.printf("ch2 :%d ", ch2);
        float LP_rc = 0.65f;
        float LP_rc3 = 0.15f;
        rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
        rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
        pc.printf("rc1 :%f ", rc1);
        pc.printf("rc2 :%f ", rc1);
        int pwmMax = 1800;
        int pwmMin = 1200;
        out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
        if(out1<pwmMin){out1 = pwmMin;};
        if(out1>pwmMax){out1 = pwmMax;};
        out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
        if(out2<pwmMin){out2 = pwmMin;};
        if(out2>pwmMax){out2 = pwmMax;};
        
        pc.printf("out1:%d ", out1);
        pc.printf("out2:%d\r\n", out2);
        servo1.pulsewidth_us(out1);
        servo2.pulsewidth_us(out2);
    }
}

void interrupt_Gyro()
{
    led3 = !led3;
}

int main()
{
    pc.baud(9600);
    servo1.period(0.020);
    servo2.period(0.020);
    LoopTicker timer_SBUS;
    LoopTicker timer_Gyro;
    timer_SBUS.attach(interrupt_SBUS, 0.020);
    timer_Gyro.attach(interrupt_Gyro, 0.5);
    while(1) {
        timer_SBUS.loop();
        timer_SBUS.loop();
    }
}