5Hz GNSS logger GNSS logging program with ublox neo7M. This utilizes ticker and timer.
Dependencies: mbed MG354PDH0 SDFileSystem
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2022-01-14
- Revision:
- 2:940851e53dde
- Parent:
- 1:8757d12d193b
- Child:
- 3:32843000531e
File content as of revision 2:940851e53dde:
//========================================================= //GNSS logger with ublox-NEO7M //MPU board: mbed LPC1768 //GNSS module: ublox-NEO7M //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(p14); // NEO-7MのCSピン DigitalIn log_switch(p15); // //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; //========================================================= //受信したメッセージから抽出したい情報 float latitude,longitude,height_float; //緯度、経度、高度 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である float velN_float,velE_float,velD_float; // NED座標系に置ける速度 //========================================================= //UBXデータを処理したかどうかのフラグ int flag_posllh,flag_velned; //========================================================= //処理時間計測の為のタイマー Timer processing_timer; //========================================================= //処理時間 int processed_time,processed_time_before,processed_time_after,measurement_time_g; //========================================================= //Ticker Ticker timer1; // Ticker timer2; // //========================================================= //Logging variables 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 }; const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 }; const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 }; const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 }; enum _ubxMsgType { MT_NONE, MT_NAV_POSLLH, MT_NAV_STATUS, MT_NAV_VELNED }; //========================================================= //メッセージの構造体 struct NAV_POSLLH { unsigned char cls; unsigned char id; unsigned short len; unsigned long iTOW; long lon; long lat; long height; long hMSL; unsigned long hAcc; unsigned long vAcc; }; struct NAV_STATUS { unsigned char cls; unsigned char id; unsigned short len; unsigned long iTOW; unsigned char gpsFix; char flags; char fixStat; char flags2; unsigned long ttff; unsigned long msss; }; struct NAV_VELNED { unsigned char cls; unsigned char id; unsigned short len; unsigned long iTOW; signed long velN; signed long velE; signed long velD; unsigned long speed; unsigned long gSpeed; signed long heading; unsigned long sAcc; unsigned long cAcc; }; //========================================================= //受信したメッセージを格納する為の共用体 union UBXMessage { NAV_VELNED navVelned;//payload size is 36bytes NAV_POSLLH navPosllh;//payload size is 28bytes NAV_STATUS navStatus;//payload size is 16bytes }; UBXMessage ubxMessage; // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid. // The procedure used to calculate this is given as pseudo-code in the uBlox manual. void calcChecksum(unsigned char* CK, int msgSize) { memset(CK, 0, 2); for (int i = 0; i < msgSize; i++) { CK[0] += ((unsigned char*)(&ubxMessage))[i]; CK[1] += CK[0]; } } //========================================================= // Compares the first two bytes of the ubxMessage struct with a specific message header. // Returns true if the two bytes match. bool compareMsgHeader(const unsigned char* msgHeader) { unsigned char* ptr = (unsigned char*)(&ubxMessage); return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1]; } //========================================================= // Reads in bytes from the GPS module and checks to see if a valid message has been constructed. // Returns the type of the message found if successful, or MT_NONE if no message was found. // After a successful return the contents of the ubxMessage union will be valid, for the // message type that was found. Note that further calls to this function can invalidate the // message content, so you must use the obtained values before calling this function again. void processGPS() { static int fpos = 0; static unsigned char checksum[2]; static unsigned char currentMsgType = MT_NONE; static int payloadSize = sizeof(UBXMessage); CS = 0; //SPIによる読み出しを開始 processed_time_before = processing_timer.read_us();// captureing prossing time /* NEO-7Mに(0xFF)を送って、取得した情報を1byteずつ以下の for文で確認する。 */ for(int buff_counter=1;buff_counter<50;buff_counter++){ unsigned char c = spi.write(0xFF); if ( fpos < 2 ) { // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62) if ( c == UBX_HEADER[fpos] ) fpos++; else fpos = 0; // Reset to beginning state. } else { // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER // and we are now reading in the bytes that make up the payload. // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because // the struct does not include the initial two-byte header (UBX_HEADER). if ( (fpos-2) < payloadSize ) ((unsigned char*)(&ubxMessage))[fpos-2] = c; fpos++; if ( fpos == 4 ) { // We have just received the second byte of the message type header, // so now we can check to see what kind of message it is. if ( compareMsgHeader(NAV_VELNED_HEADER) ) { currentMsgType = MT_NAV_VELNED; payloadSize = sizeof(NAV_VELNED); } else if ( compareMsgHeader(NAV_STATUS_HEADER) ) { currentMsgType = MT_NAV_STATUS; payloadSize = sizeof(NAV_STATUS); } else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) { currentMsgType = MT_NAV_POSLLH; payloadSize = sizeof(NAV_POSLLH); } else { // unknown message type, bail fpos = 0; continue; } } if ( fpos == (payloadSize+2) ) { // All payload bytes have now been received, so we can calculate the // expected checksum value to compare with the next two incoming bytes. calcChecksum(checksum, payloadSize); } else if ( fpos == (payloadSize+3) ) { // First byte after the payload, ie. first byte of the checksum. // Does it match the first byte of the checksum we calculated? if ( c != checksum[0] ) { // Checksum doesn't match, reset to beginning state and try again. fpos = 0; } } else if ( fpos == (payloadSize+4) ) { // Second byte after the payload, ie. second byte of the checksum. // Does it match the second byte of the checksum we calculated? fpos = 0; // We will reset the state regardless of whether the checksum matches. if ( c == checksum[1] ) { // Checksum matches, we have a valid message. if(currentMsgType==MT_NAV_POSLLH){ latitude=ubxMessage.navPosllh.lat/10000000.0f; 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; } } else if ( fpos > (payloadSize+4) ) { // We have now read more bytes than both the expected payload and checksum // together, so something went wrong. Reset to beginning state and try again. fpos = 0; } } } CS = 1; //SPIによる読み出しを終了させる //processGPS()の処理に必要な時間の計測 //複数のメッセージを読み取る、つまりこの関数をメッセージの数だけwhile内で読み出すとき、 //この関数の処理時間(processed_time)として保存されるのは //最後に呼び出されたprocessGPSの処理時間となる。 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(); 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("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); /*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); } /*--------------------------------------------*/ int main() { //UART initialization pc.baud(460800); //115.2 kbps spi.frequency(1000000); 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; //------------------------------------------- //Timer //------------------------------------------- //timer1: imu mesurement, 400 Hz timer1.attach(&imu_mesurement, imu_interval); //timer2: GNSS mesurement, 5 Hz timer2.attach(&ublox_logging, gnss_interval); 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(); break; }else if(logging_status==1){} timer2.attach(&ublox_logging, gnss_interval); wait(0.8); // }//while }