![](/media/cache/profiles/DSC_0001.jpg.50x50_q85.jpg)
UAV_Logger1から改造。サーボ機構のプログラムを追加
Dependencies: MPU6050_alter HMC5883L
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2019-05-24
- Revision:
- 2:e6496a794bde
- Parent:
- 1:9a5f06e7969e
- Child:
- 3:3fa7882a5fd0
File content as of revision 2:e6496a794bde:
#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; /*---------------------*/ /*-------ピン指定------------*/ 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); /*-----------グローバル変数-----------*/ 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); /*-------------------------------------------------------------------*/ /*プロトタイプ宣言*/ 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 } // 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); 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);//9軸の値の取得 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);//9軸の値の取得 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)); */ 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