Atsumi Toda
/
UAV_Logger1
UAVの姿勢推定に使用するプログラム。
Diff: main.cpp
- Revision:
- 2:e6496a794bde
- Parent:
- 1:9a5f06e7969e
- Child:
- 3:3fa7882a5fd0
--- a/main.cpp Thu Feb 14 10:37:28 2019 +0000 +++ b/main.cpp Fri May 24 05:57:12 2019 +0000 @@ -1,21 +1,12 @@ -/* mbed Microcontroller Library - * Copyright (c) 2006-2013 ARM Limited - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ #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" @@ -28,192 +19,482 @@ //#include "LittleFileSystem.h" #include "FATFileSystem.h" -// Physical block device, can be any device that supports the BlockDevice API -SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs +#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() { - printf("--- Mbed OS filesystem example ---\n"); + 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 - printf("Mounting the filesystem... "); + pc.printf("Mounting the filesystem... "); fflush(stdout); int err = fileSystem.mount(&blockDevice); - printf("%s\n", (err ? "Fail :(" : "OK")); + 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 - printf("No filesystem found, formatting... "); + pc.printf("No filesystem found, formatting... "); fflush(stdout); err = fileSystem.reformat(&blockDevice); - printf("%s\n", (err ? "Fail :(" : "OK")); + pc.printf("%s\n", (err ? "Fail :(" : "OK")); if (err) { error("error: %s (%d)\n", strerror(-err), err); } } // Open the numbers file - printf("Opening \"/fs/numbers.txt\"... "); + pc.printf("Opening \"/fs/numbers.txt\"... "); fflush(stdout); FILE* f = fopen("/fs/numbers.txt", "r+"); - printf("%s\n", (!f ? "Fail :(" : "OK")); + pc.printf("%s\n", (!f ? "Fail :(" : "OK")); + if (!f) { // Create the numbers file if it doesn't exist - printf("No file found, creating a new file... "); + pc.printf("No file found, creating a new file... "); fflush(stdout); f = fopen("/fs/numbers.txt", "w+"); - printf("%s\n", (!f ? "Fail :(" : "OK")); - if (!f) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - - for (int i = 0; i < 10; i++) { - printf("\rWriting numbers (%d/%d)... ", i, 10); - fflush(stdout); - err = fprintf(f, " %d\n", i); - if (err < 0) { - printf("Fail :(\n"); - error("error: %s (%d)\n", strerror(errno), -errno); + pc.printf("%s\n", (!f ? "Fail :(" : "OK")); } - } - - printf("\rWriting numbers (%d/%d)... OK\n", 10, 10); - - printf("Seeking file... "); - fflush(stdout); - err = fseek(f, 0, SEEK_SET); - printf("%s\n", (err < 0 ? "Fail :(" : "OK")); - if (err < 0) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - } - - // Go through and increment the numbers - for (int i = 0; i < 10; i++) { - printf("\rIncrementing numbers (%d/%d)... ", i, 10); - fflush(stdout); - - // Get current stream position - long pos = ftell(f); - - // Parse out the number and increment - int32_t number; - fscanf(f, "%ld", &number); - number += 1; - - // Seek to beginning of number - fseek(f, pos, SEEK_SET); - - // Store number - fprintf(f, " %ld\n", number); - - // Flush between write and read on same file - fflush(f); - } - - printf("\rIncrementing numbers (%d/%d)... OK\n", 10, 10); - - // Close the file which also flushes any cached writes - printf("Closing \"/fs/numbers.txt\"... "); - fflush(stdout); - err = fclose(f); - printf("%s\n", (err < 0 ? "Fail :(" : "OK")); - if (err < 0) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - - // Display the root directory - printf("Opening the root directory... "); - fflush(stdout); - - DIR* d = opendir("/fs/"); - printf("%s\n", (!d ? "Fail :(" : "OK")); - if (!d) { - error("error: %s (%d)\n", strerror(errno), -errno); - } + + /*初期姿勢の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 + + }; - printf("root directory:\n"); - while (true) { - struct dirent* e = readdir(d); - if (!e) { - break; - } - - printf(" %s\n", e->d_name); - } - - printf("Closing the root directory... "); - fflush(stdout); - err = closedir(d); - printf("%s\n", (err < 0 ? "Fail :(" : "OK")); - if (err < 0) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - - // Display the numbers file - printf("Opening \"/fs/numbers.txt\"... "); - fflush(stdout); - f = fopen("/fs/numbers.txt", "r"); - printf("%s\n", (!f ? "Fail :(" : "OK")); - if (!f) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - - printf("numbers:\n"); - while (!feof(f)) { - int c = fgetc(f); - printf("%c", c); - } + 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); + + /*----------------------------------------------------------------*/ - printf("\rClosing \"/fs/numbers.txt\"... "); - fflush(stdout); - err = fclose(f); - printf("%s\n", (err < 0 ? "Fail :(" : "OK")); - if (err < 0) { - error("error: %s (%d)\n", strerror(errno), -errno); - } - - // Tidy up - printf("Unmounting... "); - fflush(stdout); - err = fileSystem.unmount(); - printf("%s\n", (err < 0 ? "Fail :(" : "OK")); - if (err < 0) { - error("error: %s (%d)\n", strerror(-err), err); - } - - printf("Initializing the block device... "); - fflush(stdout); - - err = blockDevice.init(); - printf("%s\n", (err ? "Fail :(" : "OK")); - if (err) { - error("error: %s (%d)\n", strerror(-err), err); - } - - printf("Erasing the block device... "); - fflush(stdout); - err = blockDevice.erase(0, blockDevice.size()); - printf("%s\n", (err ? "Fail :(" : "OK")); - if (err) { - error("error: %s (%d)\n", strerror(-err), err); - } - - printf("Deinitializing the block device... "); - fflush(stdout); - err = blockDevice.deinit(); - printf("%s\n", (err ? "Fail :(" : "OK")); - if (err) { - error("error: %s (%d)\n", strerror(-err), err); - } - - printf("\r\n"); - - printf("Mbed OS filesystem example done!\n"); -} + 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