Atsumi Toda / Mbed 2 deprecated IMU_GPS_logger_1

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@0:847c925d2821, 2019-02-23 (annotated)

Committer:
Joeatsumi
Date:
Sat Feb 23 06:25:15 2019 +0000
Revision:
0:847c925d2821
Child:
1:9e0eb043ecdb
It enable us to log 6axis sensor datas (gyro,acceleration ) in 100Hz,ans gps datas in 10Hz.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:847c925d2821 1 /*シリアルモニタに６軸センサを100Hz,
Joeatsumi 0:847c925d2821 2 GPSからの経度、緯度、アンテナ高度、対地速度を10HZで出力する
Joeatsumi 0:847c925d2821 3 gpsからの文字列を読み取ってから６軸の値を読みだす為、出力するデータを
Joeatsumi 0:847c925d2821 4 GPGGAとGPRMCに限定しgpsモジュールを10Hz出力に設定する必要あり。
Joeatsumi 0:847c925d2821 5 sd Cardへの保存が望ましい。
Joeatsumi 0:847c925d2821 6
Joeatsumi 0:847c925d2821 7 使用部品
Joeatsumi 0:847c925d2821 8 ・LPC1768(マイコン)
Joeatsumi 0:847c925d2821 9 ・MPU6050(加速度・ジャイロセンサ)
Joeatsumi 0:847c925d2821 10 ・GPSモジュール
Joeatsumi 0:847c925d2821 11
Joeatsumi 0:847c925d2821 12 販売サイト
Joeatsumi 0:847c925d2821 13 gpsモジュール
Joeatsumi 0:847c925d2821 14
Joeatsumi 0:847c925d2821 15 */
Joeatsumi 0:847c925d2821 16
Joeatsumi 0:847c925d2821 17 #include "mbed.h"
Joeatsumi 0:847c925d2821 18 #include "MPU6050.h"
Joeatsumi 0:847c925d2821 19
Joeatsumi 0:847c925d2821 20 /*================mpu6050の温度補正係数================*/
Joeatsumi 0:847c925d2821 21 #define tempreture_coefficient_X -0.0155
Joeatsumi 0:847c925d2821 22 #define tempreture_coefficient_Y 0.0252
Joeatsumi 0:847c925d2821 23 #define tempreture_coefficient_Z -0.0147
Joeatsumi 0:847c925d2821 24 /*==========================================*/
Joeatsumi 0:847c925d2821 25
Joeatsumi 0:847c925d2821 26 /*-------ラジアン単位から度数単位への変換---------*/
Joeatsumi 0:847c925d2821 27 #define RadToDeg 57.2957914
Joeatsumi 0:847c925d2821 28 /*-------------------------------------------*/
Joeatsumi 0:847c925d2821 29
Joeatsumi 0:847c925d2821 30 /*------サンプリング間隔[s]--------*/
Joeatsumi 0:847c925d2821 31 //#define PERIOD 0.020
Joeatsumi 0:847c925d2821 32 /*------------------------------*/
Joeatsumi 0:847c925d2821 33
Joeatsumi 0:847c925d2821 34 /*---------ピン指定-------------*/
Joeatsumi 0:847c925d2821 35 MPU6050 mpu(p28, p27);//sda scl
Joeatsumi 0:847c925d2821 36 InterruptIn button(p15);
Joeatsumi 0:847c925d2821 37 DigitalOut myled(LED1);
Joeatsumi 0:847c925d2821 38 /*-----------------------------*/
Joeatsumi 0:847c925d2821 39
Joeatsumi 0:847c925d2821 40 /*-------------ポート指定-------------*/
Joeatsumi 0:847c925d2821 41 Serial pc(USBTX, USBRX); // Teratermとの接続 tx, rx
Joeatsumi 0:847c925d2821 42 Serial gps(p9,p10);// gpsとの接続 tx, rx
Joeatsumi 0:847c925d2821 43 /*-----------------------------------*/
Joeatsumi 0:847c925d2821 44 /*-----割り込み--------*/
Joeatsumi 0:847c925d2821 45 Ticker flipper;
Joeatsumi 0:847c925d2821 46 /*--------------------*/
Joeatsumi 0:847c925d2821 47 /*-----タイマー---------*/
Joeatsumi 0:847c925d2821 48 Timer passed_time;
Joeatsumi 0:847c925d2821 49 /*---------------------*/
Joeatsumi 0:847c925d2821 50 /*--------グローバル変数---------------*/
Joeatsumi 0:847c925d2821 51 float time_logger=0.0;
Joeatsumi 0:847c925d2821 52 float tempreture_initiation;//初期温度
Joeatsumi 0:847c925d2821 53
Joeatsumi 0:847c925d2821 54 //gps
Joeatsumi 0:847c925d2821 55 float g_hokui,g_tokei;
Joeatsumi 0:847c925d2821 56 float a_altitude, geoid_height;
Joeatsumi 0:847c925d2821 57 float speed_to_ground;
Joeatsumi 0:847c925d2821 58 /*----------------------------------*/
Joeatsumi 0:847c925d2821 59
Joeatsumi 0:847c925d2821 60 /*--------プロトタイプ宣言-----------*/
Joeatsumi 0:847c925d2821 61 void scan_update();
Joeatsumi 0:847c925d2821 62 void gps_rx();
Joeatsumi 0:847c925d2821 63 /*--------------------------------*/
Joeatsumi 0:847c925d2821 64
Joeatsumi 0:847c925d2821 65 void scan_update(){
Joeatsumi 0:847c925d2821 66
Joeatsumi 0:847c925d2821 67
Joeatsumi 0:847c925d2821 68 float a[3];//加速度の値
Joeatsumi 0:847c925d2821 69 float g[3];//角速度の値
Joeatsumi 0:847c925d2821 70 float tempreture_grad_X,tempreture_grad_Y,tempreture_grad_Z;//温度補正に用いる傾き
Joeatsumi 0:847c925d2821 71 float tempreture_current;//現在の温度
Joeatsumi 0:847c925d2821 72 float dpsX,dpsY,dpsZ;//角速度出力[degree/scond]
Joeatsumi 0:847c925d2821 73
Joeatsumi 0:847c925d2821 74 mpu.setAcceleroRange(1);//acceleration range is +-4G
Joeatsumi 0:847c925d2821 75 mpu.setGyroRange(1);//gyro rate is +-degree per second(dps)
Joeatsumi 0:847c925d2821 76 mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
Joeatsumi 0:847c925d2821 77 mpu.getGyro(g); // 角速度を格納する配列[rad/s]
Joeatsumi 0:847c925d2821 78
Joeatsumi 0:847c925d2821 79 dpsX = g[0]* RadToDeg;
Joeatsumi 0:847c925d2821 80 dpsY = g[1]* RadToDeg;
Joeatsumi 0:847c925d2821 81 dpsZ = g[2]* RadToDeg;
Joeatsumi 0:847c925d2821 82
Joeatsumi 0:847c925d2821 83 /*==============温度補正======================*/
Joeatsumi 0:847c925d2821 84 tempreture_current=mpu.getTemp();//温度取得
Joeatsumi 0:847c925d2821 85
Joeatsumi 0:847c925d2821 86 tempreture_grad_X=tempreture_coefficient_X*(tempreture_current-tempreture_initiation);
Joeatsumi 0:847c925d2821 87 tempreture_grad_Y=tempreture_coefficient_Y*(tempreture_current-tempreture_initiation);
Joeatsumi 0:847c925d2821 88 tempreture_grad_Z=tempreture_coefficient_Z*(tempreture_current-tempreture_initiation);
Joeatsumi 0:847c925d2821 89
Joeatsumi 0:847c925d2821 90 /*=====================================================*/
Joeatsumi 0:847c925d2821 91
Joeatsumi 0:847c925d2821 92 /*----------ジャイロ出力の温度補正----------*/
Joeatsumi 0:847c925d2821 93 dpsX-=(tempreture_grad_X);
Joeatsumi 0:847c925d2821 94 dpsY-=(tempreture_grad_Y);
Joeatsumi 0:847c925d2821 95 dpsZ-=(tempreture_grad_Z);
Joeatsumi 0:847c925d2821 96 /*--------------------------------------*/
Joeatsumi 0:847c925d2821 97
Joeatsumi 0:847c925d2821 98 double val=passed_time.read();
Joeatsumi 0:847c925d2821 99 //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);
Joeatsumi 0:847c925d2821 100 //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);
Joeatsumi 0:847c925d2821 101 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);
Joeatsumi 0:847c925d2821 102 //pc.printf("pased time is %f\r\n",val);
Joeatsumi 0:847c925d2821 103 }
Joeatsumi 0:847c925d2821 104
Joeatsumi 0:847c925d2821 105 void gps_rx(){
Joeatsumi 0:847c925d2821 106
Joeatsumi 0:847c925d2821 107 /*=============GPS信号受信に用いる変数群===============*/
Joeatsumi 0:847c925d2821 108 //char c;
Joeatsumi 0:847c925d2821 109 int i,rlock,stn;
Joeatsumi 0:847c925d2821 110 int h_time=0,m_time=0;
Joeatsumi 0:847c925d2821 111 float s_time=0;
Joeatsumi 0:847c925d2821 112 char gps_data[256];
Joeatsumi 0:847c925d2821 113 char ns,ew,av;
Joeatsumi 0:847c925d2821 114 char meter_1,meter_2;
Joeatsumi 0:847c925d2821 115 float time,hokui,tokei;
Joeatsumi 0:847c925d2821 116 // float g_hokui,g_tokei;
Joeatsumi 0:847c925d2821 117 float d_hokui,m_hokui,d_tokei,m_tokei;
Joeatsumi 0:847c925d2821 118 // float a_altitude, geoid_height;
Joeatsumi 0:847c925d2821 119 float horizontal_accuracy;
Joeatsumi 0:847c925d2821 120 //speed_to_ground
Joeatsumi 0:847c925d2821 121 float speed_knot,azi;
Joeatsumi 0:847c925d2821 122
Joeatsumi 0:847c925d2821 123
Joeatsumi 0:847c925d2821 124 /*===============================================*/
Joeatsumi 0:847c925d2821 125
Joeatsumi 0:847c925d2821 126 i=0;
Joeatsumi 0:847c925d2821 127 while(gps.getc()!='$'){
Joeatsumi 0:847c925d2821 128 }
Joeatsumi 0:847c925d2821 129 while( (gps_data[i]=gps.getc()) != '\r'){
Joeatsumi 0:847c925d2821 130 i++;
Joeatsumi 0:847c925d2821 131 if(i==256){
Joeatsumi 0:847c925d2821 132 i=255;
Joeatsumi 0:847c925d2821 133 break;
Joeatsumi 0:847c925d2821 134 }
Joeatsumi 0:847c925d2821 135 }
Joeatsumi 0:847c925d2821 136 gps_data[i]='\0';
Joeatsumi 0:847c925d2821 137
Joeatsumi 0:847c925d2821 138 //test
Joeatsumi 0:847c925d2821 139 /* Test data
Joeatsumi 0:847c925d2821 140 rlock=1;
Joeatsumi 0:847c925d2821 141 hokui=3532.25024; //=>35.537502
Joeatsumi 0:847c925d2821 142 tokei=13751.86820;//=>137.864471
Joeatsumi 0:847c925d2821 143 */
Joeatsumi 0:847c925d2821 144
Joeatsumi 0:847c925d2821 145 //////////////////////////////////////////////////////////////////////////////////////////////
Joeatsumi 0:847c925d2821 146 /*
Joeatsumi 0:847c925d2821 147 <GPSのNMEAフォーマット>
Joeatsumi 0:847c925d2821 148
Joeatsumi 0:847c925d2821 149
Joeatsumi 0:847c925d2821 150 <解説>
Joeatsumi 0:847c925d2821 151 GPSモジュールからは、NMEAフォーマットの情報が送信されます。
Joeatsumi 0:847c925d2821 152 NMEAフォーマットの情報は、センテンスの集まりです。
Joeatsumi 0:847c925d2821 153 １つのセンテンスは、「$」で始まり、「(改行(\r\n))」で終わります。
Joeatsumi 0:847c925d2821 154 センテンスは、「,」で区切られた単語の集まりです。それぞれの単語の意味は、データタイプによって異なります。
Joeatsumi 0:847c925d2821 155 センテンスの最初の単語は、データタイプを表します。
Joeatsumi 0:847c925d2821 156 センテンスの最後の単語は、「*」以降がチェックサム値を表します。
Joeatsumi 0:847c925d2821 157
Joeatsumi 0:847c925d2821 158 $GPGGA
Joeatsumi 0:847c925d2821 159 センテンス例：
Joeatsumi 0:847c925d2821 160 $GPGGA,085120.307,3541.1493,N,13945.3994,E,1,08,1.0,6.9,M,35.9,M,,0000*5E
Joeatsumi 0:847c925d2821 161
Joeatsumi 0:847c925d2821 162 085120.307 協定世界時(UTC）での時刻 time
Joeatsumi 0:847c925d2821 163 354193 緯度。dddmm.mmmm hokui
Joeatsumi 0:847c925d2821 164 N 北緯 ns
Joeatsumi 0:847c925d2821 165 13945.3994 経度 tokei
Joeatsumi 0:847c925d2821 166 E 東経 ew
Joeatsumi 0:847c925d2821 167 1 位置特定品質 rlock
Joeatsumi 0:847c925d2821 168 08 使用衛星数 stn
Joeatsumi 0:847c925d2821 169 1.0 水平精度低下率 horizontal_accuracy
Joeatsumi 0:847c925d2821 170 6.9 アンテナの海抜高さ（標高） a_altitude
Joeatsumi 0:847c925d2821 171 M 標高の単位[m] meter_1
Joeatsumi 0:847c925d2821 172 35.9 ジオイド高さ geoid_height
Joeatsumi 0:847c925d2821 173 M ジオイド高さの単位[m] meter_2
Joeatsumi 0:847c925d2821 174
Joeatsumi 0:847c925d2821 175 */
Joeatsumi 0:847c925d2821 176
Joeatsumi 0:847c925d2821 177 //////////////////////////////////////////////////////////////////////////////////////////////
Joeatsumi 0:847c925d2821 178
Joeatsumi 0:847c925d2821 179 // if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){
Joeatsumi 0:847c925d2821 180 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){
Joeatsumi 0:847c925d2821 181
Joeatsumi 0:847c925d2821 182 if(rlock >= 1){
Joeatsumi 0:847c925d2821 183
Joeatsumi 0:847c925d2821 184 myled=1;
Joeatsumi 0:847c925d2821 185 //time set
Joeatsumi 0:847c925d2821 186 h_time=int(time/10000);
Joeatsumi 0:847c925d2821 187 m_time=int((time-h_time*10000)/100);
Joeatsumi 0:847c925d2821 188 s_time=(time-h_time*10000-m_time*100);
Joeatsumi 0:847c925d2821 189 h_time=h_time+9;//UTC =>JST
Joeatsumi 0:847c925d2821 190
Joeatsumi 0:847c925d2821 191 //hokui
Joeatsumi 0:847c925d2821 192 d_hokui=int(hokui/100);
Joeatsumi 0:847c925d2821 193 m_hokui=(hokui-d_hokui*100)/60;
Joeatsumi 0:847c925d2821 194 g_hokui=d_hokui+m_hokui;
Joeatsumi 0:847c925d2821 195 //tokei
Joeatsumi 0:847c925d2821 196 d_tokei=int(tokei/100);
Joeatsumi 0:847c925d2821 197 m_tokei=(tokei-d_tokei*100)/60;
Joeatsumi 0:847c925d2821 198 g_tokei=d_tokei+m_tokei;
Joeatsumi 0:847c925d2821 199 myled=0;
Joeatsumi 0:847c925d2821 200 } //if(rlock>1)
Joeatsumi 0:847c925d2821 201 }//if sscanf
Joeatsumi 0:847c925d2821 202
Joeatsumi 0:847c925d2821 203 if( sscanf(gps_data, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f",&time,&av,&hokui,&ns,&tokei,&ew,&speed_knot,&azi) >= 1){
Joeatsumi 0:847c925d2821 204
Joeatsumi 0:847c925d2821 205 speed_to_ground=speed_knot*0.514;
Joeatsumi 0:847c925d2821 206
Joeatsumi 0:847c925d2821 207 }//if sscanf
Joeatsumi 0:847c925d2821 208 double val=passed_time.read();
Joeatsumi 0:847c925d2821 209 pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground);
Joeatsumi 0:847c925d2821 210
Joeatsumi 0:847c925d2821 211 }//void gps_rx
Joeatsumi 0:847c925d2821 212
Joeatsumi 0:847c925d2821 213 int main() {
Joeatsumi 0:847c925d2821 214
Joeatsumi 0:847c925d2821 215 //wait(20);
Joeatsumi 0:847c925d2821 216 pc.baud(230400);//シリアルポートの通信レート設定
Joeatsumi 0:847c925d2821 217 gps.baud(115200);
Joeatsumi 0:847c925d2821 218 tempreture_initiation=mpu.getTemp();//初期温度取得
Joeatsumi 0:847c925d2821 219
Joeatsumi 0:847c925d2821 220 passed_time.start();//タイマー開始
Joeatsumi 0:847c925d2821 221 //flipper.attach(&scan_update, PERIOD);//scan_update関数をPERIOD間隔で呼び出し
Joeatsumi 0:847c925d2821 222
Joeatsumi 0:847c925d2821 223 while(1) {
Joeatsumi 0:847c925d2821 224
Joeatsumi 0:847c925d2821 225 gps_rx();//4ms passed
Joeatsumi 0:847c925d2821 226 scan_update();
Joeatsumi 0:847c925d2821 227 for(int update_period=0;update_period<9;update_period++){
Joeatsumi 0:847c925d2821 228 wait_ms(6.0);
Joeatsumi 0:847c925d2821 229 scan_update();
Joeatsumi 0:847c925d2821 230 //このfor文１周で約10msとなる。
Joeatsumi 0:847c925d2821 231 }
Joeatsumi 0:847c925d2821 232
Joeatsumi 0:847c925d2821 233 }//while
Joeatsumi 0:847c925d2821 234
Joeatsumi 0:847c925d2821 235 }//main