Diff: main.cpp

Revision:

0:d22fc86a8387

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 25 16:29:38 2020 +0000
@@ -0,0 +1,326 @@
+/*
+このプログラムはNEO-7MからUBX形式でSPIを介して位置(NAV-POSLLH)、速度(NAV-VELNED)、測位状態(NAV-STATUS)を読み取るプログラムである。
+一から書き上げたものではなく、iforce2Dさんの公開しているコードを改造したものである事を強調しておきます。
+iforce2Dさんのyoutubeチャンネル
+https://www.youtube.com/user/iforce2d
+UARTでUBX情報を読み取る動画
+https://youtu.be/TwhCX0c8Xe0
+https://youtu.be/ylxwOg2pXrc
+
+SPIでu-bloxモジュールを使う際の注意として、
+
+1.SPIポートから出力されるメッセージの設定に気を付ける必要がある
+
+デフォルトではSPI通信モードの場合(D_SELピンがGND)NMEAメッセージが1Hzで出力される。
+このため、UBX情報をSPIモードで出力できるよう、u-centerでUBX-CFG(Config)-MSGで
+出力形式とメッセージの設定を行う。
+
+2.D_SELと、モジュールのパワーオンのタイミング
+「モジュールをSPIモードで使用するには、D_SELをGNDに接続する」とHardware Inregration Manualには
+記載してあるが、詳しくはD_SELをGNDに接続した後にモジュールに電源を供給しないと
+モジュールはSPIモードで動作しない。
+
+UARTでモジュールを設定
+→モジュールへの電源供給を停止
+→D_SELをGNDに接続
+→モジュールへの電源供給を再開
+
+モジュールの設定で参考になるu-bloxコミュニティ
+https://portal.u-blox.com/s/question/0D52p00008HKD2VCAX/output-ubxnavpvt-on-uart1-neo-m8p
+https://portal.u-blox.com/s/question/0D52p00008HKCStCAP/no-ubx-message-over-spi?t=1573206492449&searchQuery=
+https://portal.u-blox.com/s/question/0D52p00008HKCRhCAP/software-layer-for-spi
+
+回路の製作で参考にしたサイト
+https://space-denpa.jp/2020/05/28/neo-7m-antenna/
+*/
+
+#include "mbed.h"
+
+DigitalOut myled(LED1);
+
+// serial port シリアルポート
+Serial pc(USBTX, USBRX); // tx, rx
+// SPI通信用のポート設定
+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;
+
+//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;
+   
+}
+
+/*--------------------------------------------*/
+int main() {
+    
+    //UART initialization
+    pc.baud(460800); //115.2 kbps
+    
+    //フラグのリセット
+    flag_posllh=0;
+    flag_velned=0;
+    
+    processing_timer.start();//timer starts
+    
+    while(1) {
+        
+        /*3種類のメッセージがNEO-7Mから来るので、processGPS()は3回読み出す
+        送信されるメッセージの数だけprocessGPS()を呼び出せばよい。
+        */
+        processGPS();
+        processGPS();
+        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{}
+        
+        /*
+        while文の最後にあるwaitは、NEO7-Mを5Hzで測位させているのに合わせて設定している。
+        他のセンサの計測値を高頻度で取得させる場合、u-bloxモジュールからの情報の処理は
+        while文ではなく定期的な割り込み処理でもいいかもしれない。
+        */
+        wait(0.2);
+        
+        }//while
+  
+}
\ No newline at end of file