Atsumi Toda
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:
- 2019-02-23
- Revision:
- 0:847c925d2821
- Child:
- 1:9e0eb043ecdb
File content as of revision 0:847c925d2821:
/*シリアルモニタに６軸センサを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