Revision 1:63a0fd33d5a5, committed 2020-12-21

Comitter:

Joeatsumi

Date:

Mon Dec 21 16:49:18 2020 +0000

Parent:

0:a9095d98a2b9

Child:

2:2b6b55edb50c

Commit message:

modified ver2 2020/12/22

Changed in this revision

--- 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
   
 }