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
Diff: main.cpp
- Revision:
- 0:847c925d2821
- Child:
- 1:9e0eb043ecdb
diff -r 000000000000 -r 847c925d2821 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Feb 23 06:25:15 2019 +0000 @@ -0,0 +1,235 @@ +/*シリアルモニタに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.020 +/*------------------------------*/ + +/*---------ピン指定-------------*/ +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("%f,%f,%f,%f,%f,%f,%f,%4.6f,%3.6f,%f,%f\r\n",val,dpsX,dpsY,dpsZ,a[0],a[1],a[2],g_tokei,g_hokui,a_altitude,speed_to_ground); + //pc.printf("%7.3f,%f,%f,%f,%4.6f,%3.6f,%7.3f,%f\r\n",val,dpsX,dpsY,dpsZ,g_tokei,g_hokui,a_altitude,speed_to_ground); + 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); + //pc.printf("pased time is %f\r\n",val); + } + +void gps_rx(){ + + /*=============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); + + }//void gps_rx + +int main() { + + //wait(20); + pc.baud(230400);//シリアルポートの通信レート設定 + gps.baud(115200); + tempreture_initiation=mpu.getTemp();//初期温度取得 + + passed_time.start();//タイマー開始 + //flipper.attach(&scan_update, PERIOD);//scan_update関数をPERIOD間隔で呼び出し + + while(1) { + + gps_rx();//4ms passed + scan_update(); + for(int update_period=0;update_period<9;update_period++){ + wait_ms(6.0); + scan_update(); + //このfor文1周で約10msとなる。 + } + + }//while + +}//main \ No newline at end of file