Atsumi Toda
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
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*Atsumi Toda 2/20/2019 00002 Shibaura Institute of Technology 00003 00004 シリアルモニタに６軸センサを100Hz, 00005 GPSからの経度、緯度、アンテナ高度、対地速度を10HZで出力する 00006 gpsからの文字列を読み取ってから６軸の値を読みだす為、出力するデータを 00007 GPGGAとGPRMCに限定しgpsモジュールを10Hz出力に設定する必要あり。 00008 sd Cardへの保存が望ましい。 00009 00010 使用部品 00011 ・LPC1768(マイコン) 00012 ・MPU6050(加速度・ジャイロセンサ) 00013 ・GPSモジュール 00014 00015 販売サイト 00016 gpsモジュール 00017 00018 */ 00019 00020 #include "mbed.h" 00021 #include "MPU6050.h" 00022 00023 /*================mpu6050の温度補正係数================*/ 00024 #define tempreture_coefficient_X -0.0155 00025 #define tempreture_coefficient_Y 0.0252 00026 #define tempreture_coefficient_Z -0.0147 00027 /*==========================================*/ 00028 00029 /*-------ラジアン単位から度数単位への変換---------*/ 00030 #define RadToDeg 57.2957914 00031 /*-------------------------------------------*/ 00032 00033 /*------サンプリング間隔[s]--------*/ 00034 #define PERIOD 0.010 00035 #define GPS_UPDATE_RATE 5 00036 /*------------------------------*/ 00037 00038 /*---------ピン指定-------------*/ 00039 MPU6050 mpu(p28, p27);//sda scl 00040 InterruptIn button(p15); 00041 DigitalOut myled(LED1); 00042 /*-----------------------------*/ 00043 00044 /*-------------ポート指定-------------*/ 00045 Serial pc(USBTX, USBRX); // Teratermとの接続 tx, rx 00046 Serial gps(p9,p10);// gpsとの接続 tx, rx 00047 /*-----------------------------------*/ 00048 /*-----割り込み--------*/ 00049 Ticker flipper; 00050 /*--------------------*/ 00051 /*-----タイマー---------*/ 00052 Timer passed_time; 00053 /*---------------------*/ 00054 /*--------グローバル変数---------------*/ 00055 float time_logger=0.0; 00056 float tempreture_initiation;//初期温度 00057 00058 //gps 00059 float g_hokui,g_tokei; 00060 float a_altitude, geoid_height; 00061 float speed_to_ground; 00062 /*----------------------------------*/ 00063 00064 /*--------プロトタイプ宣言-----------*/ 00065 void scan_update(); 00066 void gps_rx(); 00067 /*--------------------------------*/ 00068 00069 void scan_update(){ 00070 00071 00072 float a[3];//加速度の値 00073 float g[3];//角速度の値 00074 float tempreture_grad_X,tempreture_grad_Y,tempreture_grad_Z;//温度補正に用いる傾き 00075 float tempreture_current;//現在の温度 00076 float dpsX,dpsY,dpsZ;//角速度出力[degree/scond] 00077 00078 mpu.setAcceleroRange(1);//acceleration range is +-4G 00079 mpu.setGyroRange(1);//gyro rate is +-degree per second(dps) 00080 mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; 00081 mpu.getGyro(g); // 角速度を格納する配列[rad/s] 00082 00083 dpsX = g[0]* RadToDeg; 00084 dpsY = g[1]* RadToDeg; 00085 dpsZ = g[2]* RadToDeg; 00086 00087 /*==============温度補正======================*/ 00088 tempreture_current=mpu.getTemp();//温度取得 00089 00090 tempreture_grad_X=tempreture_coefficient_X*(tempreture_current-tempreture_initiation); 00091 tempreture_grad_Y=tempreture_coefficient_Y*(tempreture_current-tempreture_initiation); 00092 tempreture_grad_Z=tempreture_coefficient_Z*(tempreture_current-tempreture_initiation); 00093 00094 /*=====================================================*/ 00095 00096 /*----------ジャイロ出力の温度補正----------*/ 00097 dpsX-=(tempreture_grad_X); 00098 dpsY-=(tempreture_grad_Y); 00099 dpsZ-=(tempreture_grad_Z); 00100 /*--------------------------------------*/ 00101 00102 double val=passed_time.read(); 00103 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); 00104 //pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground); 00105 //pc.printf("pased time is %f\r\n",val); 00106 } 00107 00108 void gps_rx(){ 00109 00110 __disable_irq(); // 割り込み禁止 00111 00112 /*=============GPS信号受信に用いる変数群===============*/ 00113 //char c; 00114 int i,rlock,stn; 00115 int h_time=0,m_time=0; 00116 float s_time=0; 00117 char gps_data[256]; 00118 char ns,ew,av; 00119 char meter_1,meter_2; 00120 float time,hokui,tokei; 00121 // float g_hokui,g_tokei; 00122 float d_hokui,m_hokui,d_tokei,m_tokei; 00123 // float a_altitude, geoid_height; 00124 float horizontal_accuracy; 00125 //speed_to_ground 00126 float speed_knot,azi; 00127 00128 /*===============================================*/ 00129 00130 i=0; 00131 while(gps.getc()!='$'){ 00132 } 00133 while( (gps_data[i]=gps.getc()) != '\r'){ 00134 i++; 00135 if(i==256){ 00136 i=255; 00137 break; 00138 } 00139 } 00140 gps_data[i]='\0'; 00141 00142 //test 00143 /* Test data 00144 rlock=1; 00145 hokui=3532.25024; //=>35.537502 00146 tokei=13751.86820;//=>137.864471 00147 */ 00148 00149 ////////////////////////////////////////////////////////////////////////////////////////////// 00150 /* 00151 <GPSのNMEAフォーマット> 00152 00153 00154 <解説> 00155 GPSモジュールからは、NMEAフォーマットの情報が送信されます。 00156 NMEAフォーマットの情報は、センテンスの集まりです。 00157 １つのセンテンスは、「$」で始まり、「(改行(\r\n))」で終わります。 00158 センテンスは、「,」で区切られた単語の集まりです。それぞれの単語の意味は、データタイプによって異なります。 00159 センテンスの最初の単語は、データタイプを表します。 00160 センテンスの最後の単語は、「*」以降がチェックサム値を表します。 00161 00162 $GPGGA 00163 センテンス例： 00164 $GPGGA,085120.307,3541.1493,N,13945.3994,E,1,08,1.0,6.9,M,35.9,M,,0000*5E 00165 00166 085120.307 協定世界時(UTC）での時刻 time 00167 354193 緯度。dddmm.mmmm hokui 00168 N 北緯 ns 00169 13945.3994 経度 tokei 00170 E 東経 ew 00171 1 位置特定品質 rlock 00172 08 使用衛星数 stn 00173 1.0 水平精度低下率 horizontal_accuracy 00174 6.9 アンテナの海抜高さ（標高） a_altitude 00175 M 標高の単位[m] meter_1 00176 35.9 ジオイド高さ geoid_height 00177 M ジオイド高さの単位[m] meter_2 00178 00179 */ 00180 00181 ////////////////////////////////////////////////////////////////////////////////////////////// 00182 00183 // if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){ 00184 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){ 00185 00186 if(rlock >= 1){ 00187 00188 myled=1; 00189 //time set 00190 h_time=int(time/10000); 00191 m_time=int((time-h_time*10000)/100); 00192 s_time=(time-h_time*10000-m_time*100); 00193 h_time=h_time+9;//UTC =>JST 00194 00195 //hokui 00196 d_hokui=int(hokui/100); 00197 m_hokui=(hokui-d_hokui*100)/60; 00198 g_hokui=d_hokui+m_hokui; 00199 //tokei 00200 d_tokei=int(tokei/100); 00201 m_tokei=(tokei-d_tokei*100)/60; 00202 g_tokei=d_tokei+m_tokei; 00203 myled=0; 00204 } //if(rlock>1) 00205 }//if sscanf 00206 00207 if( sscanf(gps_data, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f",&time,&av,&hokui,&ns,&tokei,&ew,&speed_knot,&azi) >= 1){ 00208 00209 speed_to_ground=speed_knot*0.514; 00210 00211 }//if sscanf 00212 double val=passed_time.read(); 00213 pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground); 00214 00215 __enable_irq(); // 割り込み許可 00216 00217 }//void gps_rx 00218 00219 int main() { 00220 00221 //wait(20); 00222 pc.baud(460800);//シリアルポートの通信レート設定 00223 gps.baud(115200); 00224 tempreture_initiation=mpu.getTemp();//初期温度取得 00225 00226 passed_time.start();//タイマー開始 00227 //flipper.attach(&scan_update, PERIOD);//scan_update関数をPERIOD間隔で呼び出し 00228 00229 while(1) { 00230 00231 gps_rx();//4ms passed 00232 wait_ms(4.0); 00233 scan_update(); 00234 for(int update_period=0;update_period< int((100/GPS_UPDATE_RATE)-1);update_period++){ 00235 wait_ms(7.6); 00236 scan_update(); 00237 //このfor文１周で約10msとなる。 00238 } 00239 00240 }//while 00241 00242 }//main
Generated on Mon Aug 15 2022 08:32:01 by
1.7.2