5Hz GNSS logger GNSS logging program with ublox neo7M. This utilizes ticker and timer.
Dependencies: mbed MG354PDH0 SDFileSystem
Diff: main.cpp
- Revision:
- 0:91d5f51f73e2
- Child:
- 1:8757d12d193b
diff -r 000000000000 -r 91d5f51f73e2 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 26 16:15:02 2021 +0000 @@ -0,0 +1,329 @@ +//========================================================= +//GNSS logger with ublox-NEO7M +//MPU board: mbed LPC1768 +//GNSS module: ublox-NEO7M +//2021/04/26 A.Toda +//======================================================== +#include "mbed.h" + +//========================================================= +//Port Setting +Serial pc(USBTX, USBRX); // tx, rx +SPI spi(p11, p12, p13); // mosi, miso, sclk +DigitalOut CS(p15); // NEO-7MのCSピン + +//========================================================= +//受信したメッセージから抽出したい情報 +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; + +//========================================================= +//Ticker +Ticker timer1; // +Ticker timer2; // + +//========================================================= +//Logging variables +float imu_mesurement_freq = 400; //Hz +float gnss_mesurement_freq = 5; //theta_update_freq; + +float imu_interval = 1.0f/imu_mesurement_freq; //sec +float gnss_interval = 1.0f/gnss_mesurement_freq; //sec + +//========================================================= +//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); + + flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる + } + else if(currentMsgType==MT_NAV_VELNED){ + velN_float=float(ubxMessage.navVelned.velN); + velE_float=float(ubxMessage.navVelned.velE); + velD_float=float(ubxMessage.navVelned.velD); + + flag_velned=1;//速度情報を読み取った合図としてフラグを立てる + + } + else if(currentMsgType==MT_NAV_STATUS){ + + } + + //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; + +} + +void imu_mesurement() +{ +} + +void ublox_logging() +{ + //detach the rotary imu mesurement + timer1.detach(); + + for(int message_number=0; message_number<3;message_number++){ + processGPS(); + } + + //位置と速度情報を読み取った場合 + 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); + //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)); + + /*フラグを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 + + //フラグのリセット + 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) { + + // + }//while + +} \ No newline at end of file