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@1:9e0eb043ecdb, 2019-02-27 (annotated)

Committer:
Joeatsumi
Date:
Wed Feb 27 03:26:21 2019 +0000
Revision:
1:9e0eb043ecdb
Parent:
0:847c925d2821 
It enables us to log 6axis sensor datas (gyro,acceleration ) in 100Hz,ans GPS datas in 5Hz. All results will be outputted in serial moniter.

Who changed what in which revision?

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