File content as of revision 37:dad55cf05e3d:

#include "mbed.h"
#include "PIDcontroller.h"
#include "SBUS.hpp"
//#include <MadgwickAHRS.h>
#include "LoopTicker.hpp"
#include "MPU6050.h"
#include <I2Cdev.h>
#include "FastPWM.h"
#include "Matrix.h"
#include "MatrixMath.h"
#include <math.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);

FastPWM servoRight(PE_9);
FastPWM servoLeft(PE_11);
FastPWM servoThrust(PA_0);
const double PID_dt = 0.015;
PID pitchPID(5.0, 0,0,PID_dt); //rad
PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
PID rollPID(5.0,0.0,0.0,PID_dt);
PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
Timer t;

Matrix qhat(4,1);
Matrix Phat(4,4);
Matrix Qgyro(3,3);
Matrix Racc(3,3);


int loop_count = 1;
float att_dt = 0.01;
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};
float scaledMotorOut[1]= {-1};
float servoOut[3] = {1500.0,1500.0};
float motorOut[1] = {1100.0};
float servoPwmMax = 1800.0;
float servoPwmMin = 1200.0;
float motorPwmMax = 2000.0;
float motorPwmMin = 1100.0;
int offsetVal[6] = {0,0,0,-600,570,265};

const double pitch_align = 0.0;
const double roll_align = -0.0;


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

void writeSdcard()
{
    pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f);
}

// 割り込まれた時点での出力(computeの結果）を返す関数
void calcServoOut(){
    if(sbus.failSafe == false) {
        // sbusデータの読み込み
        for (int i =0 ; i < 16;i ++){
            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (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]+rc[0];
    float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
    float dT = rc[2];
    
    scaledServoOut[0]=de+da;
    scaledServoOut[1]=-de+da;
    scaledMotorOut[0]= dT;
    
    double LP_servo = 0.2;
    double LP_motor = 0.2;
    for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
        if(servoOut[i]<servoPwmMin) {
            servoOut[i] = servoPwmMin;
        };
        if(servoOut[i]>servoPwmMax) {
            servoOut[i] = servoPwmMax;
        };
    }
    
    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
        if(motorOut[i]<motorPwmMin) {
            motorOut[i] = motorPwmMin;
        };
        if(motorOut[i]>motorPwmMax) {
            motorOut[i] = motorPwmMax;
        };
    }
    servoRight.pulsewidth_us(servoOut[0]);
    servoLeft.pulsewidth_us(servoOut[1]); 
    servoThrust.pulsewidth_us(motorOut[0]);
     
    if(loop_count > 10){
        writeSdcard();
        loop_count = 1;
    }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);
}

void getIMUval(){
        // gx gy gz ax ay az
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        ax = ax - offsetVal[0];
        ay = ay - offsetVal[1];
        az = az - offsetVal[2];
        gx = gx - offsetVal[3];
        gy = gy - offsetVal[4];
        gz = gz - offsetVal[5];
        // 加速度値を分解能で割って加速度(G)に変換する
        acc_x = float(ax) / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
        acc_y = float(ay) / ACCEL_SSF;
        acc_z = float(az) / ACCEL_SSF;
        // 角速度値を分解能で割って角速度(rad per sec)に変換する
        gyro_x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
        gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
        gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
}

void updateBetweenMeasures(){
        
        getIMUval();

        Matrix phi(4,4);
        phi <<  1.0               << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
            <<  gyro_x*0.5*att_dt << 1.0                << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
            <<  gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0               << gyro_x*0.5*att_dt
            <<  gyro_z*0.5*att_dt <<  gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
        qhat = phi*qhat;
        
        float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
        qhat *= (1.0f/ qnorm);
        
        float q0 = qhat.getNumber( 1, 1 );
        float q1 = qhat.getNumber( 2, 1 ); 
        float q2 = qhat.getNumber( 3, 1 ); 
        float q3 = qhat.getNumber( 4, 1 ); 
        
        Matrix B(4,3);
        B   << q1  << q2 << q3
            <<-q0  << q3 <<-q2
            <<-q3  <<-q0 << q1             
            << q2  <<-q1 <<-q0;
        B *= 0.5f;
        Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
        
        
}

void updateAcrossMeasures(){
        getIMUval();
        
        Matrix uacc(3,1);
        Matrix vacc(3,1);
        
        uacc << 0 << 0 << -1;
        float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
        vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm;
        
        float q0 = qhat.getNumber( 1, 1 );
        float q1 = qhat.getNumber( 2, 1 ); 
        float q2 = qhat.getNumber( 3, 1 ); 
        float q3 = qhat.getNumber( 4, 1 ); 
        
        Matrix A1(3,3);
        A1 << q0 << q3 << -q2
           <<-q3 << q0 << q1
           <<q2  <<-q1 <<q0;
        A1 *= 2.0f;
        
        Matrix A2(3,3);   
        A2 << q1 << q2 << q3
           << q2 <<-q1 << q0
           << q3 <<-q0 <<-q1;
        A2 *= 2.0f;
        
        Matrix A3(3,3);
        A3 <<-q2 << q1 <<-q0
           << q1 << q2 << q3
           << q0 << q3 <<-q2;
        A3 *= 2.0f;
        
        Matrix A4(3,3);
        A4 <<-q3 << q0 << q1
           <<-q0 <<-q3 << q2
           << q1 << q2 << q3;
        A4 *= 2.0f;
        
        Matrix H(3,4);

        Matrix ab1(A1*uacc);
        Matrix ab2(A2*uacc);
        Matrix ab3(A3*uacc);
        Matrix ab4(A4*uacc);

        H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
          << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
          << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
        
        Matrix D(3,3);
        D << q0*q0 + q1*q1 - q2*q2 - q3*q3
          << 2*(q1*q2 + q0*q3)
          << 2*(q1*q3 - q0*q2)
          << 2*(q1*q2 - q0*q3)
          << q0*q0 - q1*q1 + q2*q2 - q3*q3
          << 2*(q2*q3 + q0*q1)
          << 2*(q1*q3 + q0*q2)
          << 2*(q2*q3 - q0*q1)
          << q0*q0 - q1*q1 - q2*q2 + q3*q3;
        
        Matrix err(3,1);
        err = vacc-D*uacc;
        
        Matrix K(4,3);
        K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc);
        
        Matrix dq(4,1);
        dq = K*err;
        qhat = qhat-dq;
        
        float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
        qhat *= (1.0f/ qnorm);
}

void computeAngles()
{
    float q0 = qhat.getNumber( 1, 1 );
    float q1 = qhat.getNumber( 2, 1 ); 
    float q2 = qhat.getNumber( 3, 1 ); 
    float q3 = qhat.getNumber( 4, 1 ); 
    roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
    pitch = asinf(-2.0f * (q1*q3 - q0*q2));
    yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
}

int main()
{    
    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(-1.0,1.0);
    pitchPID.setInputLimits(-pi,pi);
    pitchratePID.setInputLimits(-pi,pi);
    rollPID.setInputLimits(-pi,pi); 
    rollratePID.setInputLimits(-pi,pi);
    
    servoRight.period_us(15000.0);
    servoLeft.period_us(15000.0);
    servoThrust.period_us(15000.0);
    servoRight.pulsewidth_us(1500.0);
    servoLeft.pulsewidth_us(1500.0); 
    servoThrust.pulsewidth_us(1100.0);
    
    qhat << 1 << 0 << 0 << 0;
    
    Phat << 0.0001 << 0 <<0 <<0
         << 0 << 0.0001 <<0 <<0
         << 0 << 0 <<0.0001 <<0
         << 0 << 0 << 0 << 0.0001;
    
    Qgyro << 0.0001 << 0 <<0
          << 0 << 0.0001 <<0
          << 0 << 0 <<0.0001;     
    
    Racc << 0.0001 << 0 <<0
         << 0 << 0.0001 <<0
         << 0 << 0 <<0.0001;
    
    
    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);
    //MadgwickFilter.begin(100); //サンプリング周波数Hza
    t.start();
    //servoRight.write(0.0f);
    while(1) {
        float tstart = t.read();
        //姿勢角を更新
        //updateAttitude();
        updateBetweenMeasures();
        updateAcrossMeasures();
        computeAngles();
        PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
        
        float tend = t.read();
        att_dt = (tend-tstart);
        //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz
        
    }
}