File content as of revision 15:6abaac6e5b03:

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

SBUS sbus(PD_5, PD_6);
MPU6050 mpu(PB_9,PB_8);
Serial pc(USBTX, USBRX);
IMUfilter filter(1/100, 0.1);
DigitalOut led1(LED1);
DigitalOut led3(LED3);
PwmOut servo1(PB_4);
PwmOut servo2(PB_5);

int ch1, ch2;
float rc1, rc2;
double pitch = 0.0;
double roll = 0.0;
double yaw = 0.0;
float gyro[3] = {0,0,0};
float acc[3] = {0,0,1.0};
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;};
    }
    pitch = filter.getPitch();
    
    pc.printf("pitch:%f ", pitch);
    pc.printf("out1:%d ", out1);
    pc.printf("out2:%d\r\n", out2);
    servo1.pulsewidth_us(out1);
    servo2.pulsewidth_us(out2);
}

void interrupt_Gyro()
{
    // gx gy gz ax ay az
    mpu.getGyro(gyro);
    printf("gyro: %f\n",gyro[0]);
    mpu.getAccelero(acc);
    printf("acc: %f\n",acc[0]);
    filter.updateFilter(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]);
}

int main()
{
    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
    mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
    pc.baud(9600);
    servo1.period(0.020);
    servo2.period(0.020);
    LoopTicker timer_SBUS;
    LoopTicker timer_Gyro;
    timer_SBUS.attach(interrupt_SBUS, 1/50);
    timer_Gyro.attach(interrupt_Gyro, 1/100);
    while(1) {
        timer_SBUS.loop();
        timer_Gyro.loop();
    }
}