UBX decode program that is modified from i2dforce UBX program for NAV-STATUS and NAV POSLLH. I used NEO-7M and mbed LPC1768.
Diff: main.cpp
- Revision:
- 1:63a0fd33d5a5
- Parent:
- 0:a9095d98a2b9
- Child:
- 2:2b6b55edb50c
--- a/main.cpp Fri Dec 18 02:44:01 2020 +0000 +++ b/main.cpp Mon Dec 21 16:49:18 2020 +0000 @@ -1,21 +1,38 @@ -#include "mbed.h" +/* +このプログラムはNEO-7MからUBX形式でUARTを介して位置、速度(NAV-VELNED)、測位状態(NAV-STATUS)を読み取るプログラムである。 +このプログラムには欠点があり、NAV-POSSLHとNAV-VELNEDを同時に読み取る事が出来ない点である。 +具体的には。1Hzでその3つの情報を +*/ -//NAV- +#include "mbed.h" DigitalOut myled(LED1); // serial port Serial pc(USBTX, USBRX); // tx, rx -Serial ublox_ubx(p13,p14);//tx,rx +Serial ublox_ubx(p9,p10);//tx,rx +//variables for decodeing +float latitude,longitude,height_float; +int gps_Fix; +float velN_float,velE_float,velD_float; + +int flag_posllh,flag_velned; +// + +//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_STATUS, + MT_NAV_VELNED }; struct NAV_POSLLH { @@ -44,9 +61,26 @@ 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_POSLLH navPosllh; - NAV_STATUS navStatus; + NAV_VELNED navVelned;//payload size is 36bytes + NAV_POSLLH navPosllh;//payload size is 28bytes + NAV_STATUS navStatus;//payload size is 16bytes }; UBXMessage ubxMessage; @@ -76,6 +110,7 @@ // 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. int processGPS() { + static int fpos = 0; static unsigned char checksum[2]; @@ -108,14 +143,23 @@ 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_POSLLH_HEADER) ) { - currentMsgType = MT_NAV_POSLLH; - payloadSize = sizeof(NAV_POSLLH); + + 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; @@ -142,6 +186,23 @@ 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; } } @@ -155,18 +216,28 @@ return MT_NONE; } -float latitude,longitude,height_float; -int gps_Fix; - /*--------------------------------------------*/ int main() { //UART initialization - pc.baud(460800); //115.2 kbps - ublox_ubx.baud(460800); //115.2 kbps + pc.baud(115200); //115.2 kbps + ublox_ubx.baud(115200); //115.2 kbps + + flag_posllh=0; + flag_velned=0; while(1) { int msgType = processGPS(); + + if((flag_posllh==1)&&(flag_velned==1)){ + 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); + + flag_posllh=0; + flag_velned=0; + } + else{} + /* if ( msgType == MT_NAV_POSLLH ) { latitude=ubxMessage.navPosllh.lat/10000000.0f; longitude=ubxMessage.navPosllh.lon/10000000.0f; @@ -178,6 +249,16 @@ gps_Fix=int(ubxMessage.navStatus.gpsFix); pc.printf("gps_Fix=%d\r\n",gps_Fix); } - } + else if ( msgType == 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); + //Tera termでNED座標系での速度履歴を保存したい場合、以下のコメント出力を用いる。 + pc.printf("%f,%f,%f\r\n",velN_float,velE_float,velD_float); + } + */ + + }//while }