6軸センサ(ジャイロ、加速度)を100Hz, GPSの数値を10Hzでロギングする。結果は全てシリアルモニタに出力される。 It enables us to log 6axis sensor datas (gyro,acceleration ) in 100Hz,ans GPS datas in 10Hz. All results will be outputted in serial moniter.
Dependencies: mbed MPU6050_alter
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2019-02-27
- Revision:
- 1:9e0eb043ecdb
- Parent:
- 0:847c925d2821
File content as of revision 1:9e0eb043ecdb:
/*Atsumi Toda 2/20/2019 Shibaura Institute of Technology シリアルモニタに6軸センサを100Hz, GPSからの経度、緯度、アンテナ高度、対地速度を10HZで出力する gpsからの文字列を読み取ってから6軸の値を読みだす為、出力するデータを GPGGAとGPRMCに限定しgpsモジュールを10Hz出力に設定する必要あり。 sd Cardへの保存が望ましい。 使用部品 ・LPC1768(マイコン) ・MPU6050(加速度・ジャイロセンサ) ・GPSモジュール 販売サイト gpsモジュール */ #include "mbed.h" #include "MPU6050.h" /*================mpu6050の温度補正係数================*/ #define tempreture_coefficient_X -0.0155 #define tempreture_coefficient_Y 0.0252 #define tempreture_coefficient_Z -0.0147 /*==========================================*/ /*-------ラジアン単位から度数単位への変換---------*/ #define RadToDeg 57.2957914 /*-------------------------------------------*/ /*------サンプリング間隔[s]--------*/ #define PERIOD 0.010 #define GPS_UPDATE_RATE 5 /*------------------------------*/ /*---------ピン指定-------------*/ MPU6050 mpu(p28, p27);//sda scl InterruptIn button(p15); DigitalOut myled(LED1); /*-----------------------------*/ /*-------------ポート指定-------------*/ Serial pc(USBTX, USBRX); // Teratermとの接続 tx, rx Serial gps(p9,p10);// gpsとの接続 tx, rx /*-----------------------------------*/ /*-----割り込み--------*/ Ticker flipper; /*--------------------*/ /*-----タイマー---------*/ Timer passed_time; /*---------------------*/ /*--------グローバル変数---------------*/ float time_logger=0.0; float tempreture_initiation;//初期温度 //gps float g_hokui,g_tokei; float a_altitude, geoid_height; float speed_to_ground; /*----------------------------------*/ /*--------プロトタイプ宣言-----------*/ void scan_update(); void gps_rx(); /*--------------------------------*/ void scan_update(){ float a[3];//加速度の値 float g[3];//角速度の値 float tempreture_grad_X,tempreture_grad_Y,tempreture_grad_Z;//温度補正に用いる傾き float tempreture_current;//現在の温度 float dpsX,dpsY,dpsZ;//角速度出力[degree/scond] mpu.setAcceleroRange(1);//acceleration range is +-4G mpu.setGyroRange(1);//gyro rate is +-degree per second(dps) mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; mpu.getGyro(g); // 角速度を格納する配列[rad/s] dpsX = g[0]* RadToDeg; dpsY = g[1]* RadToDeg; dpsZ = g[2]* RadToDeg; /*==============温度補正======================*/ tempreture_current=mpu.getTemp();//温度取得 tempreture_grad_X=tempreture_coefficient_X*(tempreture_current-tempreture_initiation); tempreture_grad_Y=tempreture_coefficient_Y*(tempreture_current-tempreture_initiation); tempreture_grad_Z=tempreture_coefficient_Z*(tempreture_current-tempreture_initiation); /*=====================================================*/ /*----------ジャイロ出力の温度補正----------*/ dpsX-=(tempreture_grad_X); dpsY-=(tempreture_grad_Y); dpsZ-=(tempreture_grad_Z); /*--------------------------------------*/ double val=passed_time.read(); pc.printf("A,%7.3f,%f,%f,%f,%f,%f,%f,%f\r\n",val,dpsX,dpsY,dpsZ,a[0],a[1],a[2],tempreture_current,g_tokei,g_hokui,a_altitude,speed_to_ground); //pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground); //pc.printf("pased time is %f\r\n",val); } void gps_rx(){ __disable_irq(); // 割り込み禁止 /*=============GPS信号受信に用いる変数群===============*/ //char c; int i,rlock,stn; int h_time=0,m_time=0; float s_time=0; char gps_data[256]; char ns,ew,av; char meter_1,meter_2; float time,hokui,tokei; // float g_hokui,g_tokei; float d_hokui,m_hokui,d_tokei,m_tokei; // float a_altitude, geoid_height; float horizontal_accuracy; //speed_to_ground float speed_knot,azi; /*===============================================*/ i=0; while(gps.getc()!='$'){ } while( (gps_data[i]=gps.getc()) != '\r'){ i++; if(i==256){ i=255; break; } } gps_data[i]='\0'; //test /* Test data rlock=1; hokui=3532.25024; //=>35.537502 tokei=13751.86820;//=>137.864471 */ ////////////////////////////////////////////////////////////////////////////////////////////// /* <GPSのNMEAフォーマット> <解説> GPSモジュールからは、NMEAフォーマットの情報が送信されます。 NMEAフォーマットの情報は、センテンスの集まりです。 1つのセンテンスは、「$」で始まり、「(改行(\r\n))」で終わります。 センテンスは、「,」で区切られた単語の集まりです。それぞれの単語の意味は、データタイプによって異なります。 センテンスの最初の単語は、データタイプを表します。 センテンスの最後の単語は、「*」以降がチェックサム値を表します。 $GPGGA センテンス例: $GPGGA,085120.307,3541.1493,N,13945.3994,E,1,08,1.0,6.9,M,35.9,M,,0000*5E 085120.307 協定世界時(UTC)での時刻 time 354193 緯度。dddmm.mmmm hokui N 北緯 ns 13945.3994 経度 tokei E 東経 ew 1 位置特定品質 rlock 08 使用衛星数 stn 1.0 水平精度低下率 horizontal_accuracy 6.9 アンテナの海抜高さ(標高) a_altitude M 標高の単位[m] meter_1 35.9 ジオイド高さ geoid_height M ジオイド高さの単位[m] meter_2 */ ////////////////////////////////////////////////////////////////////////////////////////////// // if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){ if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn,&horizontal_accuracy,&a_altitude,&meter_1,&geoid_height,&meter_2) >= 1){ if(rlock >= 1){ myled=1; //time set h_time=int(time/10000); m_time=int((time-h_time*10000)/100); s_time=(time-h_time*10000-m_time*100); h_time=h_time+9;//UTC =>JST //hokui d_hokui=int(hokui/100); m_hokui=(hokui-d_hokui*100)/60; g_hokui=d_hokui+m_hokui; //tokei d_tokei=int(tokei/100); m_tokei=(tokei-d_tokei*100)/60; g_tokei=d_tokei+m_tokei; myled=0; } //if(rlock>1) }//if sscanf if( sscanf(gps_data, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f",&time,&av,&hokui,&ns,&tokei,&ew,&speed_knot,&azi) >= 1){ speed_to_ground=speed_knot*0.514; }//if sscanf double val=passed_time.read(); pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground); __enable_irq(); // 割り込み許可 }//void gps_rx int main() { //wait(20); pc.baud(460800);//シリアルポートの通信レート設定 gps.baud(115200); tempreture_initiation=mpu.getTemp();//初期温度取得 passed_time.start();//タイマー開始 //flipper.attach(&scan_update, PERIOD);//scan_update関数をPERIOD間隔で呼び出し while(1) { gps_rx();//4ms passed wait_ms(4.0); scan_update(); for(int update_period=0;update_period< int((100/GPS_UPDATE_RATE)-1);update_period++){ wait_ms(7.6); scan_update(); //このfor文1周で約10msとなる。 } }//while }//main