File content as of revision 3:79e62f9b13c8:

#include "mbed.h"
#include "SBUS.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);

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

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

void interrupt_SBUS()
{
    led1 = !led1;
    if(sbus.failSafe == false)
    {
        ch1_b = ch1;
        ch2_b = ch2;
        ch1 = int(sbus.getData(1));
        ch2 = int(sbus.getData(2));
        pc.printf("ch1 :%d\r\n", ch1);
        pc.printf("ch2 :%d\r\n", ch1);
        if(abs(ch1-ch1_b) < 100){
          rc1 = (float)(map(ch1,368,1680,-1000,1000))/1000.0;
        }
        if(abs(ch2-ch2_b) < 100){
          rc2 = (float)(map(ch2,368,1680,-1000,1000))/1000.0;
        }
        pc.printf("rc1 :%f\r\n", rc1);
        pc.printf("rc2 :%f\r\n", rc1);
        int pwmMax = 1800;
        int pwmMin = 1200;
        out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax);
        if(out1<pwmMin){out1 = pwmMin;};
        if(out1>pwmMax){out1 = pwmMax;};
        out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax);
        if(out2<pwmMin){out2 = pwmMin;};
        if(out2>pwmMax){out2 = pwmMax;};
        
        pc.printf("out1:%d\r\n", 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);
    Ticker timer_SBUS;
    Ticker timer_Gyro;
    timer_SBUS.attach(interrupt_SBUS, 1.0);
    timer_Gyro.attach(interrupt_Gyro, 0.1);
    while(1) {
        
    }
}