Diff: main.cpp

Revision:

0:847c925d2821

Child:

1:9e0eb043ecdb

--- /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 @@
+/*シリアルモニタに６軸センサを100Hz,
+GPSからの経度、緯度、アンテナ高度、対地速度を10HZで出力する
+gpsからの文字列を読み取ってから６軸の値を読みだす為、出力するデータを
+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フォーマットの情報は、センテンスの集まりです。
+１つのセンテンスは、「$」で始まり、「(改行(\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文１周で約10msとなる。    
+            }
+        
+    }//while
+    
+}//main 
\ No newline at end of file