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

Files at this revision

API Documentation at this revision

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

Changed in this revision

MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Sat Feb 23 06:25:15 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Joeatsumi/code/MPU6050_alter/#44c458576810
--- /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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Feb 23 06:25:15 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file