5Hz GNSS logger GNSS logging program with ublox neo7M. This utilizes ticker and timer.
Dependencies: mbed MG354PDH0 SDFileSystem
Diff: main.cpp
- Revision:
- 1:8757d12d193b
- Parent:
- 0:91d5f51f73e2
- Child:
- 2:940851e53dde
--- a/main.cpp Mon Apr 26 16:15:02 2021 +0000 +++ b/main.cpp Wed Apr 28 06:59:04 2021 +0000 @@ -5,12 +5,20 @@ //2021/04/26 A.Toda //======================================================== #include "mbed.h" - +#include "SDFileSystem.h" //========================================================= //Port Setting Serial pc(USBTX, USBRX); // tx, rx SPI spi(p11, p12, p13); // mosi, miso, sclk DigitalOut CS(p15); // NEO-7MのCSピン +DigitalIn log_switch(p16); // + +//SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd"); +SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs + +//ファイルポインタ +FILE *fp; +FILE *im; //========================================================= //受信したメッセージから抽出したい情報 @@ -28,7 +36,7 @@ //========================================================= //処理時間 -int processed_time,processed_time_before,processed_time_after; +int processed_time,processed_time_before,processed_time_after,measurement_time_g; //========================================================= //Ticker @@ -37,12 +45,13 @@ //========================================================= //Logging variables -float imu_mesurement_freq = 400; //Hz -float gnss_mesurement_freq = 5; //theta_update_freq; +float imu_mesurement_freq = 200.0; //Hz +float gnss_mesurement_freq = 5.0; //theta_update_freq; float imu_interval = 1.0f/imu_mesurement_freq; //sec float gnss_interval = 1.0f/gnss_mesurement_freq; //sec +int logging_status; //========================================================= //Header char const unsigned char UBX_HEADER[] = { 0xB5, 0x62 }; @@ -227,20 +236,27 @@ longitude=ubxMessage.navPosllh.lon/10000000.0f; height_float=float(ubxMessage.navPosllh.height); + //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); + + flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる + //pc.printf("flag_posllh=%d\r\n",flag_posllh); + } else if(currentMsgType==MT_NAV_VELNED){ velN_float=float(ubxMessage.navVelned.velN); velE_float=float(ubxMessage.navVelned.velE); velD_float=float(ubxMessage.navVelned.velD); + //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float); flag_velned=1;//速度情報を読み取った合図としてフラグを立てる + //pc.printf("flag_velned=%d\r\n",flag_velned); } else if(currentMsgType==MT_NAV_STATUS){ } - + else{} //return currentMsgType; } } @@ -262,40 +278,60 @@ processed_time_after = processing_timer.read_us();// captureing prossing time processed_time=processed_time_after-processed_time_before; + /*processGPSの処理時間の表示*/ + //pc.printf("processed_time_after(us)=%d;",(processed_time_after)); + //pc.printf("processed_time(us)=%d\r\n",(processed_time)); + //pc.printf("%d,%d\r\n",processed_time_after,processed_time); + } void imu_mesurement() { + + if(log_switch==1){ + logging_status=1; + }else if(log_switch==0){ + logging_status=0; + }else{} + } void ublox_logging() { //detach the rotary imu mesurement timer1.detach(); - - for(int message_number=0; message_number<3;message_number++){ - processGPS(); - } + + processGPS(); + processGPS(); + processGPS(); + + measurement_time_g = processing_timer.read_us(); + //pc.printf("%d\r\n",measurement_time_g);// captureing prossing time + if(logging_status==1){ + fprintf(fp, "%d,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float); + }else if(logging_status==0){} + //位置と速度情報を読み取った場合 if((flag_posllh==1)&&(flag_velned==1)){ /*Teratermでロギングする用の表示*/ - pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float); + //pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float); /*計測ではなくデバッグ用の表示*/ //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float); - //pc.printf("processed_time(us)=%d\r\n",processed_time); - + + /*processGPSの処理時間の表示*/ //pc.printf("processed_time_before(us)=%d\r\n",(processed_time_before)); //pc.printf("processed_time_after(us)=%d\r\n",(processed_time_after)); + //pc.printf("processed_time(us)=%d\r\n",processed_time); /*フラグを0に戻す*/ flag_posllh=0; flag_velned=0; }else{} - + //attach a timer for imu mesurement (400 Hz) timer1.attach(&imu_mesurement, imu_interval); } @@ -307,6 +343,21 @@ //UART initialization pc.baud(460800); //115.2 kbps + spi.frequency(5500000); + + mkdir("/sd/mydir",0777);//SDファイル作成 + fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外で行う + //im = fopen("/sd/mydir/imu.txt", "a"); + + if(fp == NULL) { + error("Could not open file for write\n"); + }else{} + + pc.printf("FO\r\n");//file open + logging_status=1; + + wait(0.1); + //フラグのリセット flag_posllh=0; flag_velned=0; @@ -322,7 +373,18 @@ processing_timer.start();//timer starts while(1) { + pc.printf("T2D\r\n"); + timer2.detach(); + if(logging_status==0){ + fclose(fp); + pc.printf("FC\r\n"); + timer2.detach(); + timer1.detach(); + }else if(logging_status==1){} + + timer2.attach(&ublox_logging, gnss_interval); + wait(0.8); // }//while