File content as of revision 3:3fa7882a5fd0:

#include "mbed.h"
#include <stdio.h>
#include <errno.h>
#include "MPU6050.h"
#include "HMC5883L.h"

#include "Vector.h"
#include "Matrix.h"
#include "Vector_Matrix_operator.h"

// Block devices
//#include "SPIFBlockDevice.h"
//#include "DataFlashBlockDevice.h"
#include "SDBlockDevice.h"
//#include "HeapBlockDevice.h"

// File systems

//#include "LittleFileSystem.h"
#include "FATFileSystem.h"

#define M_PI 3.141592
#define PI2  (2*M_PI)

/*地磁気センサの補正値（足し合わせる）*/
#define MAG_FIX_X 338
#define MAG_FIX_Y 20
#define MAG_FIX_Z -50

/*ジャイロセンサの補正値（引く）*/
#define GYRO_FIX_X 290.5498
#define GYRO_FIX_Y 99.29363
#define GYRO_FIX_Z 61.41011

// File system declaration
FATFileSystem   fileSystem("fs");

/*-----割り込み--------*/
Ticker flipper;
/*--------------------*/
/*-----タイマー---------*/
Timer passed_time;
/*---------------------*/
/*-------ピン指定------------*/
//declare PWM pins here
PwmOut ESC (p21);
PwmOut Elevator_servo(p22);
PwmOut Rudder_servo(p23);


SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs
MPU6050 mpu(p28, p27);//sda scl
HMC5883L compass(p28, p27);//sda scl

RawSerial gps(p9,p10); //serial port for gps_module
RawSerial pc(USBTX, USBRX);

/*--------------------------*/

DigitalIn switch_off(p30);
DigitalOut led2(LED2);

/*-----------グローバル変数-----------*/
double Euler_angle[3];
double Euler_angle_Initiated[3];

float g_hokui,g_tokei;
int fp_count=0;
int gps_flag=0;
int gps_flag_conversion=0;

char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している

unsigned int  tokei_int_receive;
unsigned int  hokui_int_receive;

float  tokei_float_receive;
float  hokui_float_receive;

float a[3];//加速度の値
float g[3];//ジャイロの値[rad/s]
float dpsX,dpsY,dpsZ;        
float AccX,AccY,AccZ;

int sensor_array[6];
int16_t mag[3];

double Roll_Acc,Pitch_Acc;
double Yaw;
double Azi_mag;//地磁気からの方位
double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion

/*----------------------------------*/
/*--------------------------行列、ベクトル-----------------------------*/

Vector Quaternion_atitude_from_omega(4);
Matrix Omega_matrix(4,4),Half_matrix(4,4);
Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);

/*-------------------------------------------------------------------*/

/*----------PID コントロール-----------------*/
#define Gain_Kp 0.8377
#define Gain_Ki  0.4450
#define Gain_Kd  0.3890
 
double Prop_p,Int_p,Dif_p;
double Pre_Prop_p;
/*-----------------------------------------*/

/*-----------サーボコントロール----------------*/
//define period of servo here
#define SERVO_PERIOD 0.020 // servo requires a 20ms period
#define NUTRAL  0.0015
#define UPPER_DUTY  0.0020
#define LOWER_DUTY  0.0010

double pitch_duty,roll_duty,yaw_duty;

/*------------------------------------------*/


/*プロトタイプ宣言*/
void toString_V(Vector& v);   // ベクトルデバッグ用
void gps_rx();
void gps_convertion();
/*--------------*/

void toString_V(Vector& v)
{

    for(int i=0; i<v.GetDim(); i++) {
        pc.printf("%.6f\t", v.GetComp(i+1));
    }
    pc.printf("\r\n");

}

void scan_update(int box_sensor[6],int16_t m[3]){
    
    int a[3];//加速度の値
    int g[3];//角速度の値

    int16_t raw_compass[3];//地磁気センサの値
 
    mpu.setAcceleroRange(0);//acceleration range is +-4G
    mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
    
    mpu.getAcceleroRaw(a);//   加速度を格納する配列[m/s2]  a[0] is x axis,a[1] is y axis,a[2] is z axis;
    mpu.getGyroRaw(g);    //   角速度を格納する配列[rad/s] 
    compass.getXYZ(raw_compass);//地磁気の値を格納する配列
    
    box_sensor[0]=-g[0];
    box_sensor[1]=g[1];
    box_sensor[2]=-g[2];
    
    box_sensor[3]=a[0];
    box_sensor[4]=-a[1];
    box_sensor[5]=a[2];
    
    m[0]=(raw_compass[0]);//x
    m[1]=(raw_compass[2]);//y
    m[2]=(raw_compass[1]);//z

}

void gps_rx(){
            
            if((gps.getc()=='$')&&(gps_flag==0)){
               for(int i=0;i<9;i++){
                    gps_data[i]=gps.getc();
                    //pc.putc(gps_data[i]);
                    }
                    
                gps_data[9]='\0';
                
               for(int i=0;i<9;i++){
                    gps_data_2[i]=gps.getc();
                    //pc.putc(gps_data[i]);
                    }
                    
                gps_data_2[9]='\0';
                
                }//if(twe.getc()==':')
            
            //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
            
            gps_flag=1;
            
    }//gps_rx ends

void gps_convertion(){
    while(1){
        if(gps_flag==1){
            tokei_int_receive=atoi(gps_data);
                hokui_int_receive=atoi(gps_data_2);
                
                //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
                
                tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
                hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;   
                
                gps_flag_conversion=1;
                
            }else{gps_flag_conversion=0;}
        }//while ends
    
    }//ends

void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
    
    double Roll_before = double(y_acc)/double(z_acc);
    double Roll = atan(Roll_before);
             
    double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
    double Pitch = -atan(Pitch_before);
             
    //Yaw=0.0;
    
    /*Quaternionの要素へオイラー角を変換する*/
    qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
    qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
    qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0);
    qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0);
    
             
    }
    
void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
    
    double Roll_before = double(y_acc)/double(z_acc);
    double Roll = atan(Roll_before);
             
    double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
    double Pitch = -atan(Pitch_before);

    
    qua[0]=Pitch;
    qua[1]=Roll;
         
    }

void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
    
    //y_acc*=-1.0;
    
    n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
    n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
    n[2]=0.0;
    
    n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
    
    }

double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
    
    Vector Mag_from_sensor(3),Mag_fixed_sensor(3);                    //座標系
    Matrix Rotation(3, 3);                        //検算の行列

    double Correct_vector_n[4];//回転行列の成分
    Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
                                 
    double n_x=Correct_vector_n[0];
    double n_y=Correct_vector_n[1];
    double n_z=Correct_vector_n[2];
    double theta=-Correct_vector_n[3];
                                 
    float Rotate_element[9] = {
        n_x*n_x*(1-cos(theta))+cos(theta)    , n_x*n_y*(1-cos(theta))       , n_y*sin(theta),  
        n_x*n_y*(1-cos(theta))                ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta), 
        n_y*sin(theta)                       , n_x*sin(theta)              , cos(theta)
    };
                
    Rotation.SetComps(Rotate_element);
                                                //x           y                    z
    float Geomagnetism[3]={  float(magnet[1]+MAG_FIX_X) 
                           , float(magnet[0]+MAG_FIX_Y)  
                           , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
                           
    Mag_from_sensor.SetComps(Geomagnetism);
                
    Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
                
    float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
    float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
       
    double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
    
    /*            
    if(Azi < 0.0) // fix sign
    Azi += PI2;
            
    if(Azi > PI2) // fix overflow
    Azi -= PI2;
    */
    
    return Azi;
    
    }

//void led2_thread(void const *argument) {
void imu_thread() {
    while (true) {
    }//while ends
}

void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){
    
    //float pitch,roll,yaw;
    
    Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) );
    Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2)));
    Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3));

    }
    

double Degree_to_PWM_duty(double degree){
    
    double duty=0.0;
    
    duty=(0.2/1000)/20.0*degree;
    
    if((duty+NUTRAL)>=UPPER_DUTY){
        duty=UPPER_DUTY-NUTRAL;
    }else if((duty+NUTRAL)<=LOWER_DUTY){
        duty=NUTRAL-LOWER_DUTY;
    }else{}
    
    return duty;
    
    }
    
double PID_duty(double target,double vol,float dt){
            //ここで角度の単位で偏差は入力される
            //出力はPWMで
            double duty=0.0;
            double add_duty=0.0;
            
            Prop_p=target-vol;
            
            pc.printf("%f/r/n",Prop_p);
            
            Int_p+=Prop_p*double(dt);
            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
            
            Pre_Prop_p=Prop_p;
            
            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
            
            add_duty=1.0*Prop_p;
            
            duty+=add_duty;
            duty=Degree_to_PWM_duty(duty);
            
            return duty;
            
}

void Initialize_ESC(){
    
    ESC.pulsewidth(0.002);
    wait(3);
    
    ESC.pulsewidth(0.001);
    wait(5);
    
    }
    
    
void Activate_ESC(){
    
    ESC.pulsewidth(0.001);
    wait(5);    
    
    }

// Entry point for the example
int main()
{
    gps.baud(115200);//GT-720Fボーレート
    pc.baud(115200);
    //imu_thread
    
    compass.init();//hmc5883の起動
    
    Thread thread1 (gps_convertion);
    gps.attach(gps_rx, Serial::RxIrq);
    
        /*define period of serve and ESC*/
    ESC.period(SERVO_PERIOD);    
    Elevator_servo.period(SERVO_PERIOD);
    Rudder_servo.period(SERVO_PERIOD);
    /*------------------------------*/
    Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
    Rudder_servo.pulsewidth(NUTRAL);
    
    Activate_ESC();
    
    float Time_old=0.0;//時間初期化
    passed_time.start();//タイマー開始
    
    pc.printf("--- Mbed OS filesystem example ---\n");

    // Try to mount the filesystem
    pc.printf("Mounting the filesystem... ");
    fflush(stdout);

    int err = fileSystem.mount(&blockDevice);
    pc.printf("%s\n", (err ? "Fail :(" : "OK"));
    if (err) {
        // Reformat if we can't mount the filesystem
        // this should only happen on the first boot
        pc.printf("No filesystem found, formatting... ");
        fflush(stdout);
        err = fileSystem.reformat(&blockDevice);
        pc.printf("%s\n", (err ? "Fail :(" : "OK"));
        if (err) {
            error("error: %s (%d)\n", strerror(-err), err);
        }
    }

    // Open the numbers file
    pc.printf("Opening \"/fs/numbers.txt\"... ");
    fflush(stdout);

    FILE*   f = fopen("/fs/numbers.txt", "r+");
    pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
    
    if (!f) {
        // Create the numbers file if it doesn't exist
        pc.printf("No file found, creating a new file... ");
        fflush(stdout);
        f = fopen("/fs/numbers.txt", "w+");
        pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
            }
    
    /*初期姿勢のQuaternionの設定*/
    scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x  mag[2]=-Z
    Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
    
    Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
    
    float Initialize_atitude[4];
    Initialize_atitude[0]=Quaternion_from_acc[0];
    Initialize_atitude[1]=Quaternion_from_acc[1];
    Initialize_atitude[2]=Quaternion_from_acc[2];
    Initialize_atitude[3]=Quaternion_from_acc[3];
    
    Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
    
    
    while(1){
 
            /*gpsから来た文字列の処理
            if((gps_flag==1)&&(gps_flag_conversion==1)){
                
                err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
                 
                gps_flag=0;
                gps_flag_conversion=0;
                
                }else{}
            */
                
            scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x  mag[2]=-Z
            float Time_new=float(passed_time.read());//時間の取得
            
            //地磁気の補正用に使う。
            //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
            
            //ジャイロの補正用に使う
            //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
            
            /*----------------ジャイロセンサからQuaternionを求める----------------*/
            float omega_x=float((  float(sensor_array[0])-GYRO_FIX_X  )/ 7505.7);
            float omega_y=float((  float(sensor_array[1])-GYRO_FIX_Y  )/ 7505.7);
            float omega_z=float((  float(sensor_array[2])-GYRO_FIX_Z  )/ 7505.7);
            
            float omega[16] = {
            
            0.0         , -omega_x , -omega_y, -omega_z,
            omega_x     ,0.0       ,omega_z  ,-omega_y ,
            omega_y     , -omega_z , 0.0     , omega_x , 
            omega_z     ,  omega_y ,-omega_x ,   0.0    
                
                 };

            Omega_matrix.SetComps(omega);
            
            float half[16] ={
                (Time_new-Time_old)*0.5,0.0,0.0,0.0,
                0.0,(Time_new-Time_old)*0.5,0.0,0.0,
                0.0,0.0,(Time_new-Time_old)*0.5,0.0,
                0.0,0.0,0.0,(Time_new-Time_old)*0.5
                };
                
            
            Half_matrix.SetComps(half);
            //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
            Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
            
            Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
            
            
            //Time_old=Time_new;//時間の更新
            
            /*
            pc.printf("%f,%f,%f,%f\r\n"
                ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
                ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
            */
            
            /*----------------------------------------------------------------*/
            
            /*
            err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
                ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
                    ,sensor_array[6],sensor_array[7],sensor_array[8]);
            */
            
            /*--------------------加速センサで地磁気の値を補正する --------------------*/
            
            if(9.8065*1.1  >  
            sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
                        / 16384.0 * 9.81)
            {
                
                Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
                    
                //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);                
                
                double pitch_and_roll[2];
                Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
                
                //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG);
                
                Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
                
                /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
                float Quaternion_from_acc_fixed[4];
                Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
                Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
                Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
                Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
                 
                Vector Qua_acc(4);
                 
                Qua_acc.SetComps(Quaternion_from_acc_fixed);                
                /*---------------------------------------------*/
                
                /*---------------------相補フィルタのゲインに用いる---------------------*/
                float comp_1[16]={0.95,0.0,0.0,0.0,
                                 0.0,0.95,0.0,0.0,
                                 0.0,0.0,0.95,0.0,
                                 0.0,0.0,0.0,0.95
                                };
                /*--------------------------------------------------------------------*/
                /*---------------------quaternionの時間微分に用いる---------------------*/
                float comp_2[16]={0.05,0.0,0.0,0.0,
                                 0.0,0.05,0.0,0.0,
                                 0.0,0.0,0.05,0.0,
                                 0.0,0.0,0.0,0.05
                                };
                                
                Complement_matrix_1.SetComps(comp_1);
                Complement_matrix_2.SetComps(comp_2);
                
                /*--------------------------------------------------------------------*/
                
                                
                /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
                
                Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
                                    +Complement_matrix_2*Qua_acc;
                
                /*----------------------------------------------------------------------------*/
                
                
                /*
                pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
                     ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
                     ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
                     ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
                     ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
                */
                     
            }else{}
            
            
            /*
            pc.printf("%f,%f,%f,%f,%f\r\n"
                     ,Time_new
                     ,Quaternion_atitude_from_omega.GetComp(1)
                     ,Quaternion_atitude_from_omega.GetComp(2)
                     ,Quaternion_atitude_from_omega.GetComp(3)
                     ,Quaternion_atitude_from_omega.GetComp(4));
            */
            /*-----------Quaternionからオイラー角への変換-----------*/
            Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1)
                                ,Quaternion_atitude_from_omega.GetComp(2)
                                ,Quaternion_atitude_from_omega.GetComp(3)
                                ,Quaternion_atitude_from_omega.GetComp(4)
                                ,Euler_angle);
                                
            /*-----------オイラー角の表示----------------------*/
            
            pc.printf("%f,%f,%f,%f,%f\r\n"
                     ,Time_new
                     ,Euler_angle[0]
                     ,Euler_angle[1]
                     ,Euler_angle[2]
                     );
            
            /*--------------------------------------------------*/
            //Pitch control
            pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old));
            Elevator_servo.pulsewidth(pitch_duty); 
             
             
            Time_old=Time_new;//時間の更新
            
            wait(0.001);
            
            /*----------------------------------------------------------------*/

            if(switch_off==1){
                // Close the file which also flushes any cached writes
                    pc.printf("Closing \"/fs/numbers.txt\"... ");
                    err = fclose(f);
                    break;
                }
            
            }//while(1) ends
}//main ends