File content as of revision 28:fc6c46c1db65:

#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
#define ACCEL_FSR MPU6050_ACCEL_FS_8
#define ACCEL_SSF 36500.0
#define GYRO_FSR MPU6050_GYRO_FS_250
#define GYRO_SSF 131.0
#define MPU6050_LPF MPU6050_DLPF_BW_256

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);
const double PID_dt = 0.01;
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 loop_count = 0;
float rc[16];
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 scaledServoOut[3]= {0,0,0};
float scaledMotorOut[1]= {-1};
int servoOut[3] = {1500,1500,1500};
int motorOut[1] = {1000};
int servoPwmMax = 1800;
int servoPwmMin = 1200;
int motorPwmMax = 1800;
int motorPwmMin = 1200;
int offsetVal[6] = {0,0,0,0,0,0};

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("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll);
}

// 割り込まれた時点での出力(computeの結果）を返す関数
void calcServoOut(){
    if(sbus.failSafe == false) {
        // sbusデータの読み込み
        for (int i =0 ; i < 16;i ++){
            rc[i] = 0.65f * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - 0.65f) * rc[i]; // mapped input
        }
    }
    
    pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
    roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
    pitchPID.setProcessValue(pitch);
    pitchratePID.setProcessValue(gyro_y);
    rollPID.setProcessValue(roll);
    rollratePID.setProcessValue(gyro_x);
    float de = pitchPID.compute()+pitchratePID.compute()+rc[1];
    float da = rollPID.compute()+rollratePID.compute()+rc[0];
    float dT = rc[2];
    
    scaledServoOut[0]=de+da;
    scaledServoOut[1]=de-da;
    scaledMotorOut[0]= dT;
    for(int i = 0;i<sizeof(servoOut);i++){
        servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))
        if(servoOut[i]<servoPwmMin) {
            servoOut[i] = servoPwmMin;
        };
        if(servoOut[i]>servoPwmMax) {
            servoOut[i] = servoPwmMax;
        };
    }
    
    for(int i = 0;i<sizeof(motorOut);i++){
        motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))
        if(motorOut[i]<motorPwmMin) {
            motorOut[i] = motorPwmMin;
        };
        if(motorOut[i]>motorPwmMax) {
            motorOut[i] = motorPwmMax;
        };
    }
    
    servoRight.pulsewidth_us(servoOut[0]);
    //servoLeft.pulsewidth_us(servoOut[1]); 
        
    if(loop_count > 10)
        pushto_sdcard();
        loop_count = 0;
    }else{
        loop_count +=1;
    }
}

void updateAttitude(){
        // 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）を計算
        MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
}

int main()
{
    LoopTicker sdcard_loop;
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    pc.baud(115200);
    sd.baud(115200);
    accelgyro.initialize();
    //加速度計のフルスケールレンジを設定
    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
    //■角速度計のフルスケールレンジを設定
    accelgyro.setFullScaleGyroRange(GYRO_FSR);
    //MPU6050のLPFを設定
    accelgyro.setDLPFMode(MPU6050_LPF);
    
    pitchPID.setSetPoint(0.0);
    pitchratePID.setSetPoint(0.0); 
    rollPID.setSetPoint(0.0); 
    rollratePID.setSetPoint(0.0); 
    pitchPID.setBias(0.0);
    pitchratePID.setBias(0.0); 
    rollPID.setBias(0.0); 
    rollratePID.setBias(0.0); 
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchratePID.setOutputLimits(-1.0,1.0);
    rollPID.setOutputLimits(-1.0,1.0); 
    rollratePID.setOutputLimits(-pi,pi);
    pitchPID.setInputLimits(-pi,pi);
    pitchratePID.setInputLimits(-pi,pi);
    rollPID.setInputLimits(-pi,pi); 
    rollratePID.setInputLimits(-pi,pi);
    servoRight.period_us(20000);
    MadgwickFilter.begin(100); //サンプリング周波数Hza
    t.start();
    while(1) {
        tstart = t.read();
        //姿勢角を更新
        updateAttitude()
        PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
        
        float tend = t.read();
        pc.printf("time%f \r\n", (tend-tstart));
        MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz
        
    }
}