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

Committer:
Joeatsumi
Date:
2 months ago
Revision:
1:9e0eb043ecdb
Parent:
0:847c925d2821

File content as of revision 1:9e0eb043ecdb:

/*Atsumi Toda  2/20/2019
Shibaura Institute of Technology 

シリアルモニタに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.010
#define GPS_UPDATE_RATE 5
/*------------------------------*/

/*---------ピン指定-------------*/
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("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);
    //pc.printf("G,%7.3f,%4.6f,%3.6f,%7.3f,%f\r\n",val,g_tokei,g_hokui,a_altitude,speed_to_ground);
    //pc.printf("pased time is %f\r\n",val);
    }
    
void gps_rx(){

       __disable_irq(); // 割り込み禁止
        
    /*=============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);
    
    __enable_irq(); // 割り込み許可
    
    }//void gps_rx
        
int main() {
    
    //wait(20);
    pc.baud(460800);//シリアルポートの通信レート設定
    gps.baud(115200);
    tempreture_initiation=mpu.getTemp();//初期温度取得
    
    passed_time.start();//タイマー開始
    //flipper.attach(&scan_update, PERIOD);//scan_update関数をPERIOD間隔で呼び出し        
           
    while(1) {
        
        gps_rx();//4ms passed
        wait_ms(4.0);
        scan_update(); 
        for(int update_period=0;update_period< int((100/GPS_UPDATE_RATE)-1);update_period++){
              wait_ms(7.6);
            scan_update();
            //このfor文1周で約10msとなる。    
            }
        
    }//while
    
}//main