File content as of revision 26:62d9857eaecc:

#include "mbed.h"
#include "PIDcontroller.h"
#include "SBUS.hpp"
#include <MadgwickAHRS.h>
#include "LoopTicker.hpp"
#include "MPU6050.h"
#include <I2Cdev.h>

#define MPU6050_PWR_MGMT_1   0x6B
#define MPU_ADDRESS  0x68
#define pi 3.141562

//加速度 Full-Scale Range
//#define ACCEL_FSR MPU6050_ACCEL_FS_2
//#define ACCEL_FSR MPU6050_ACCEL_FS_4
#define ACCEL_FSR MPU6050_ACCEL_FS_8
//#define ACCEL_FSR MPU6050_ACCEL_FS_16
//加速度 Sensitivity Scale Factor
#define ACCEL_SSF 36500.0
//角速度 Full-Scale Range
#define GYRO_FSR MPU6050_GYRO_FS_250
//#define GYRO_FSR MPU6050_GYRO_FS_500
//#define GYRO_FSR MPU6050_GYRO_FS_1000
//#define GYRO_FSR MPU6050_GYRO_FS_2000
//角速度 Sensitivity Scale Factor
#define GYRO_SSF 131.0
//#define GYRO_SSF 65.5
//#define GYRO_SSF 32.8
//#define GYRO_SSF 16.4
#define MPU6050_LPF MPU6050_DLPF_BW_256
//#define MPU6050_LPF MPU6050_DLPF_BW_188
//#define MPU6050_LPF MPU6050_DLPF_BW_98
//#define MPU6050_LPF MPU6050_DLPF_BW_42
//#define MPU6050_LPF MPU6050_DLPF_BW_20
//#define MPU6050_LPF MPU6050_DLPF_BW_10
//#define MPU6050_LPF MPU6050_DLPF_BW_5
MPU6050 accelgyro;
Madgwick MadgwickFilter;
SBUS sbus(PD_5, PD_6);
Serial pc(USBTX, USBRX);
Serial sd(PG_14,PG_9);
DigitalOut led1(LED1);
DigitalOut led3(LED3);
PwmOut servoRight(PE_9);
//PwmOut co(PE_9); // サーボのoutput
//Servo servoRight(PE_); //右のサーボ
//Servo servoLeft(PB_5); //左のサーボ
//Servo thrServo(PB_6); //スロットルサーボ

// のセットアップ
// (float Kp, float Ki, float Kd, float tSample);
const double PID_dt = 0.01;
PID test_control(1.2, 0.0, 0.0, PID_dt);
PID pitchPID(1.0, 0,0,PID_dt); //rad
PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
PID rollPID(1.0,0.0,0.0,PID_dt);
PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
Timer t;

int ch1, ch2;
float rc[16];
//float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ
float roll_ac;
double pitch = 0.0;
double roll = 0.0;
double yaw = 0.0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
double acc_x,acc_y,acc_z;
double gyro_x,gyro_y,gyro_z;
int out1, out2;
float scaled_servo[3];
int servo_out[3];

const double pitch_align = 0.0;
const double roll_align = -0.0;
float tstart;
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 pushto_sdcard()
{
    //sd.printf("tstart: %f \n",tstart);
    sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll);
}
// 割り込まれた時点での出力(computeの結果）を返す関数
int loop_count = 0;
void PID_compute(){
    scaled_servo[0] = test_control.compute();
    loop_count +=1;
    if(loop_count == 10)    {
        pc.printf("log:%d",loop_count);
        pushto_sdcard();
        loop_count = 0;
    }
}

int main()
{
    LoopTicker sdcard_loop;
    LoopTicker PIDtick;
    PIDtick.attach(PID_compute,PID_dt);
    sdcard_loop.attach(pushto_sdcard,1.0);
    pc.baud(115200);
    sd.baud(115200);
    accelgyro.initialize();
    uint8_t offsetVal[6] = {0,0,0,0,0,0};
    accelgyro.setXAccelOffset(offsetVal[0]);
    accelgyro.setYAccelOffset(offsetVal[1]);
    accelgyro.setZAccelOffset(offsetVal[2]);
    accelgyro.setXGyroOffset(offsetVal[3]);
    accelgyro.setYGyroOffset(offsetVal[4]);
    accelgyro.setZGyroOffset(offsetVal[5]);
    //加速度計のフルスケールレンジを設定
    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
    //■角速度計のフルスケールレンジを設定
    accelgyro.setFullScaleGyroRange(GYRO_FSR);
    //MPU6050のLPFを設定
    accelgyro.setDLPFMode(MPU6050_LPF);

    servoRight.period_us(20000);

    MadgwickFilter.begin(100); //サンプリング周波数Hza
    // のセットアップ
    test_control.setInputLimits(-60.0,60.0); // 60°が限界
    test_control.setOutputLimits(-1, 1);
    test_control.setBias(0.0);
    t.start();
    float LP_rc = 0.65f;
    while(1) {
        tstart = t.read();
        if(sbus.failSafe == false) {
            // sbusデータの読み込み
            for (int i =0 ; i < 16;i ++){
                rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input
            }
            float rc_setpoint = map((int)(rc[1]*1000.0f),-1000,1000,-30,30); // sbus用
            pc.printf("data: %f",rc_setpoint);
            test_control.setSetPoint(rc_setpoint);
        }
        else test_control.setSetPoint(0.0);

        // gx gy gz ax ay az
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        // 加速度値を分解能で割って加速度(G)に変換する
        acc_x = ax / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
        acc_y = ay / ACCEL_SSF;
        acc_z = az / ACCEL_SSF;
        // 角速度値を分解能で割って角速度(deg per sec)に変換する
        gyro_x = gx / GYRO_SSF;  // (deg/s)
        gyro_y = gy / GYRO_SSF;
        gyro_z = gz / GYRO_SSF;
        //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
        //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z);
        MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
        
        pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
        roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
        
        //float LP_rc3 = 0.15f;
        roll_ac = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac;
        int pwmMax = 1800;
        int pwmMin = 1200;
        //out1 = map((int)（rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
        if(out1<pwmMin) {
            out1 = pwmMin;
        };
        if(out1>pwmMax) {
            out1 = pwmMax;
        };
        out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
        if(out2<pwmMin) {
            out2 = pwmMin;
        };
        if(out2>pwmMax) {
            out2 = pwmMax;
        };
        pc.printf("roll%lf ", gyro_x);
        //pc.printf("out1:%d\n ", out1);
        //pc.printf("out2:%d ", out2);
        wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
        float tend = t.read();
        MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
        pc.printf("time%f \r\n", (tend-tstart));
        test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
        PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
        //sdcard_loop.loop();
        printf("scaled_servo[0]: %f",scaled_servo[0]);
        servo_out[0] = map((int)(scaled_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
        for (int i = 0; i < 3 ; i ++){
            servo_out[i] = max(servo_out[i],pwmMin);
            servo_out[i] = min(servo_out[i], pwmMax);
        }
        pc.printf("out1:%d ",servo_out[0]);
        servoRight.pulsewidth_us(servo_out[0]); 
    }
}