Atsumi Toda / Mbed 2 deprecated UBX_GPS_muliti_SPI_3

This program reads the position (NAV-POSLLH), velocity (NAV-VELNED), and positioning status (NAV-STATUS) from the NEO-7M via SPI in UBX format. This is not my original program, but a modification of iforce2's program so that it can be executed via SPI communication. His original program is explained in the following video. https://youtu.be/TwhCX0c8Xe0 https://youtu.be/ylxwOg2pXrc I used the following website to create the circuit. https://space-denpa.jp/2020/05/28/neo-7m-antenna/

Dependencies:   mbed

main.cpp@0:d22fc86a8387, 2020-12-25 (annotated)

Committer:
Joeatsumi
Date:
Fri Dec 25 16:29:38 2020 +0000
Revision:
0:d22fc86a8387
ver1 20201226

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:d22fc86a8387 1 /*
Joeatsumi 0:d22fc86a8387 2 このプログラムはNEO-7MからUBX形式でSPIを介して位置(NAV-POSLLH)、速度(NAV-VELNED)、測位状態(NAV-STATUS)を読み取るプログラムである。
Joeatsumi 0:d22fc86a8387 3 一から書き上げたものではなく、iforce2Dさんの公開しているコードを改造したものである事を強調しておきます。
Joeatsumi 0:d22fc86a8387 4 iforce2Dさんのyoutubeチャンネル
Joeatsumi 0:d22fc86a8387 5 https://www.youtube.com/user/iforce2d
Joeatsumi 0:d22fc86a8387 6 UARTでUBX情報を読み取る動画
Joeatsumi 0:d22fc86a8387 7 https://youtu.be/TwhCX0c8Xe0
Joeatsumi 0:d22fc86a8387 8 https://youtu.be/ylxwOg2pXrc
Joeatsumi 0:d22fc86a8387 9
Joeatsumi 0:d22fc86a8387 10 SPIでu-bloxモジュールを使う際の注意として、
Joeatsumi 0:d22fc86a8387 11
Joeatsumi 0:d22fc86a8387 12 1.SPIポートから出力されるメッセージの設定に気を付ける必要がある
Joeatsumi 0:d22fc86a8387 13
Joeatsumi 0:d22fc86a8387 14 デフォルトではSPI通信モードの場合(D_SELピンがGND)NMEAメッセージが1Hzで出力される。
Joeatsumi 0:d22fc86a8387 15 このため、UBX情報をSPIモードで出力できるよう、u-centerでUBX-CFG(Config)-MSGで
Joeatsumi 0:d22fc86a8387 16 出力形式とメッセージの設定を行う。
Joeatsumi 0:d22fc86a8387 17
Joeatsumi 0:d22fc86a8387 18 2.D_SELと、モジュールのパワーオンのタイミング
Joeatsumi 0:d22fc86a8387 19 「モジュールをSPIモードで使用するには、D_SELをGNDに接続する」とHardware Inregration Manualには
Joeatsumi 0:d22fc86a8387 20 記載してあるが、詳しくはD_SELをGNDに接続した後にモジュールに電源を供給しないと
Joeatsumi 0:d22fc86a8387 21 モジュールはSPIモードで動作しない。
Joeatsumi 0:d22fc86a8387 22
Joeatsumi 0:d22fc86a8387 23 UARTでモジュールを設定
Joeatsumi 0:d22fc86a8387 24 →モジュールへの電源供給を停止
Joeatsumi 0:d22fc86a8387 25 →D_SELをGNDに接続
Joeatsumi 0:d22fc86a8387 26 →モジュールへの電源供給を再開
Joeatsumi 0:d22fc86a8387 27
Joeatsumi 0:d22fc86a8387 28 モジュールの設定で参考になるu-bloxコミュニティ
Joeatsumi 0:d22fc86a8387 29 https://portal.u-blox.com/s/question/0D52p00008HKD2VCAX/output-ubxnavpvt-on-uart1-neo-m8p
Joeatsumi 0:d22fc86a8387 30 https://portal.u-blox.com/s/question/0D52p00008HKCStCAP/no-ubx-message-over-spi?t=1573206492449&searchQuery=
Joeatsumi 0:d22fc86a8387 31 https://portal.u-blox.com/s/question/0D52p00008HKCRhCAP/software-layer-for-spi
Joeatsumi 0:d22fc86a8387 32
Joeatsumi 0:d22fc86a8387 33 回路の製作で参考にしたサイト
Joeatsumi 0:d22fc86a8387 34 https://space-denpa.jp/2020/05/28/neo-7m-antenna/
Joeatsumi 0:d22fc86a8387 35 */
Joeatsumi 0:d22fc86a8387 36
Joeatsumi 0:d22fc86a8387 37 #include "mbed.h"
Joeatsumi 0:d22fc86a8387 38
Joeatsumi 0:d22fc86a8387 39 DigitalOut myled(LED1);
Joeatsumi 0:d22fc86a8387 40
Joeatsumi 0:d22fc86a8387 41 // serial port シリアルポート
Joeatsumi 0:d22fc86a8387 42 Serial pc(USBTX, USBRX); // tx, rx
Joeatsumi 0:d22fc86a8387 43 // SPI通信用のポート設定
Joeatsumi 0:d22fc86a8387 44 SPI spi(p11, p12, p13); // mosi, miso, sclk
Joeatsumi 0:d22fc86a8387 45 DigitalOut CS(p15); // NEO-7MのCSピン
Joeatsumi 0:d22fc86a8387 46
Joeatsumi 0:d22fc86a8387 47 //受信したメッセージから抽出したい情報
Joeatsumi 0:d22fc86a8387 48 float latitude,longitude,height_float; //緯度、経度、高度
Joeatsumi 0:d22fc86a8387 49 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である
Joeatsumi 0:d22fc86a8387 50 float velN_float,velE_float,velD_float; // NED座標系に置ける速度
Joeatsumi 0:d22fc86a8387 51
Joeatsumi 0:d22fc86a8387 52 //UBXデータを処理したかどうかのフラグ
Joeatsumi 0:d22fc86a8387 53 int flag_posllh,flag_velned;
Joeatsumi 0:d22fc86a8387 54
Joeatsumi 0:d22fc86a8387 55 //処理時間計測の為のタイマー
Joeatsumi 0:d22fc86a8387 56 Timer processing_timer;
Joeatsumi 0:d22fc86a8387 57 //処理時間
Joeatsumi 0:d22fc86a8387 58 int processed_time,processed_time_before,processed_time_after;
Joeatsumi 0:d22fc86a8387 59
Joeatsumi 0:d22fc86a8387 60 //Header char
Joeatsumi 0:d22fc86a8387 61 const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
Joeatsumi 0:d22fc86a8387 62 const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
Joeatsumi 0:d22fc86a8387 63 const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 };
Joeatsumi 0:d22fc86a8387 64
Joeatsumi 0:d22fc86a8387 65 const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 };
Joeatsumi 0:d22fc86a8387 66
Joeatsumi 0:d22fc86a8387 67 enum _ubxMsgType {
Joeatsumi 0:d22fc86a8387 68 MT_NONE,
Joeatsumi 0:d22fc86a8387 69 MT_NAV_POSLLH,
Joeatsumi 0:d22fc86a8387 70 MT_NAV_STATUS,
Joeatsumi 0:d22fc86a8387 71 MT_NAV_VELNED
Joeatsumi 0:d22fc86a8387 72 };
Joeatsumi 0:d22fc86a8387 73 /*メッセージの構造体*/
Joeatsumi 0:d22fc86a8387 74 struct NAV_POSLLH {
Joeatsumi 0:d22fc86a8387 75 unsigned char cls;
Joeatsumi 0:d22fc86a8387 76 unsigned char id;
Joeatsumi 0:d22fc86a8387 77 unsigned short len;
Joeatsumi 0:d22fc86a8387 78 unsigned long iTOW;
Joeatsumi 0:d22fc86a8387 79 long lon;
Joeatsumi 0:d22fc86a8387 80 long lat;
Joeatsumi 0:d22fc86a8387 81 long height;
Joeatsumi 0:d22fc86a8387 82 long hMSL;
Joeatsumi 0:d22fc86a8387 83 unsigned long hAcc;
Joeatsumi 0:d22fc86a8387 84 unsigned long vAcc;
Joeatsumi 0:d22fc86a8387 85 };
Joeatsumi 0:d22fc86a8387 86
Joeatsumi 0:d22fc86a8387 87 struct NAV_STATUS {
Joeatsumi 0:d22fc86a8387 88 unsigned char cls;
Joeatsumi 0:d22fc86a8387 89 unsigned char id;
Joeatsumi 0:d22fc86a8387 90 unsigned short len;
Joeatsumi 0:d22fc86a8387 91 unsigned long iTOW;
Joeatsumi 0:d22fc86a8387 92 unsigned char gpsFix;
Joeatsumi 0:d22fc86a8387 93 char flags;
Joeatsumi 0:d22fc86a8387 94 char fixStat;
Joeatsumi 0:d22fc86a8387 95 char flags2;
Joeatsumi 0:d22fc86a8387 96 unsigned long ttff;
Joeatsumi 0:d22fc86a8387 97 unsigned long msss;
Joeatsumi 0:d22fc86a8387 98 };
Joeatsumi 0:d22fc86a8387 99
Joeatsumi 0:d22fc86a8387 100 struct NAV_VELNED {
Joeatsumi 0:d22fc86a8387 101 unsigned char cls;
Joeatsumi 0:d22fc86a8387 102 unsigned char id;
Joeatsumi 0:d22fc86a8387 103 unsigned short len;
Joeatsumi 0:d22fc86a8387 104 unsigned long iTOW;
Joeatsumi 0:d22fc86a8387 105 signed long velN;
Joeatsumi 0:d22fc86a8387 106 signed long velE;
Joeatsumi 0:d22fc86a8387 107 signed long velD;
Joeatsumi 0:d22fc86a8387 108 unsigned long speed;
Joeatsumi 0:d22fc86a8387 109 unsigned long gSpeed;
Joeatsumi 0:d22fc86a8387 110 signed long heading;
Joeatsumi 0:d22fc86a8387 111 unsigned long sAcc;
Joeatsumi 0:d22fc86a8387 112 unsigned long cAcc;
Joeatsumi 0:d22fc86a8387 113
Joeatsumi 0:d22fc86a8387 114 };
Joeatsumi 0:d22fc86a8387 115
Joeatsumi 0:d22fc86a8387 116 //受信したメッセージを格納する為の共用体
Joeatsumi 0:d22fc86a8387 117 union UBXMessage {
Joeatsumi 0:d22fc86a8387 118 NAV_VELNED navVelned;//payload size is 36bytes
Joeatsumi 0:d22fc86a8387 119 NAV_POSLLH navPosllh;//payload size is 28bytes
Joeatsumi 0:d22fc86a8387 120 NAV_STATUS navStatus;//payload size is 16bytes
Joeatsumi 0:d22fc86a8387 121 };
Joeatsumi 0:d22fc86a8387 122
Joeatsumi 0:d22fc86a8387 123 UBXMessage ubxMessage;
Joeatsumi 0:d22fc86a8387 124
Joeatsumi 0:d22fc86a8387 125 // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid.
Joeatsumi 0:d22fc86a8387 126 // The procedure used to calculate this is given as pseudo-code in the uBlox manual.
Joeatsumi 0:d22fc86a8387 127 void calcChecksum(unsigned char* CK, int msgSize) {
Joeatsumi 0:d22fc86a8387 128 memset(CK, 0, 2);
Joeatsumi 0:d22fc86a8387 129 for (int i = 0; i < msgSize; i++) {
Joeatsumi 0:d22fc86a8387 130 CK[0] += ((unsigned char*)(&ubxMessage))[i];
Joeatsumi 0:d22fc86a8387 131 CK[1] += CK[0];
Joeatsumi 0:d22fc86a8387 132 }
Joeatsumi 0:d22fc86a8387 133 }
Joeatsumi 0:d22fc86a8387 134
Joeatsumi 0:d22fc86a8387 135 // Compares the first two bytes of the ubxMessage struct with a specific message header.
Joeatsumi 0:d22fc86a8387 136 // Returns true if the two bytes match.
Joeatsumi 0:d22fc86a8387 137 bool compareMsgHeader(const unsigned char* msgHeader) {
Joeatsumi 0:d22fc86a8387 138 unsigned char* ptr = (unsigned char*)(&ubxMessage);
Joeatsumi 0:d22fc86a8387 139 return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
Joeatsumi 0:d22fc86a8387 140 }
Joeatsumi 0:d22fc86a8387 141
Joeatsumi 0:d22fc86a8387 142 // Reads in bytes from the GPS module and checks to see if a valid message has been constructed.
Joeatsumi 0:d22fc86a8387 143 // Returns the type of the message found if successful, or MT_NONE if no message was found.
Joeatsumi 0:d22fc86a8387 144 // After a successful return the contents of the ubxMessage union will be valid, for the
Joeatsumi 0:d22fc86a8387 145 // message type that was found. Note that further calls to this function can invalidate the
Joeatsumi 0:d22fc86a8387 146 // message content, so you must use the obtained values before calling this function again.
Joeatsumi 0:d22fc86a8387 147 void processGPS() {
Joeatsumi 0:d22fc86a8387 148
Joeatsumi 0:d22fc86a8387 149 static int fpos = 0;
Joeatsumi 0:d22fc86a8387 150 static unsigned char checksum[2];
Joeatsumi 0:d22fc86a8387 151
Joeatsumi 0:d22fc86a8387 152 static unsigned char currentMsgType = MT_NONE;
Joeatsumi 0:d22fc86a8387 153 static int payloadSize = sizeof(UBXMessage);
Joeatsumi 0:d22fc86a8387 154
Joeatsumi 0:d22fc86a8387 155 CS = 0; //SPIによる読み出しを開始
Joeatsumi 0:d22fc86a8387 156
Joeatsumi 0:d22fc86a8387 157 processed_time_before = processing_timer.read_us();// captureing prossing time
Joeatsumi 0:d22fc86a8387 158
Joeatsumi 0:d22fc86a8387 159 /*
Joeatsumi 0:d22fc86a8387 160 NEO-7Mに(0xFF)を送って、取得した情報を1byteずつ以下の
Joeatsumi 0:d22fc86a8387 161 for文で確認する。
Joeatsumi 0:d22fc86a8387 162
Joeatsumi 0:d22fc86a8387 163 */
Joeatsumi 0:d22fc86a8387 164 for(int buff_counter=1;buff_counter<50;buff_counter++){
Joeatsumi 0:d22fc86a8387 165
Joeatsumi 0:d22fc86a8387 166 unsigned char c = spi.write(0xFF);
Joeatsumi 0:d22fc86a8387 167
Joeatsumi 0:d22fc86a8387 168 if ( fpos < 2 ) {
Joeatsumi 0:d22fc86a8387 169 // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62)
Joeatsumi 0:d22fc86a8387 170 if ( c == UBX_HEADER[fpos] )
Joeatsumi 0:d22fc86a8387 171 fpos++;
Joeatsumi 0:d22fc86a8387 172 else
Joeatsumi 0:d22fc86a8387 173 fpos = 0; // Reset to beginning state.
Joeatsumi 0:d22fc86a8387 174 }
Joeatsumi 0:d22fc86a8387 175 else {
Joeatsumi 0:d22fc86a8387 176 // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER
Joeatsumi 0:d22fc86a8387 177 // and we are now reading in the bytes that make up the payload.
Joeatsumi 0:d22fc86a8387 178
Joeatsumi 0:d22fc86a8387 179 // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because
Joeatsumi 0:d22fc86a8387 180 // the struct does not include the initial two-byte header (UBX_HEADER).
Joeatsumi 0:d22fc86a8387 181 if ( (fpos-2) < payloadSize )
Joeatsumi 0:d22fc86a8387 182 ((unsigned char*)(&ubxMessage))[fpos-2] = c;
Joeatsumi 0:d22fc86a8387 183
Joeatsumi 0:d22fc86a8387 184
Joeatsumi 0:d22fc86a8387 185 fpos++;
Joeatsumi 0:d22fc86a8387 186
Joeatsumi 0:d22fc86a8387 187 if ( fpos == 4 ) {
Joeatsumi 0:d22fc86a8387 188 // We have just received the second byte of the message type header,
Joeatsumi 0:d22fc86a8387 189 // so now we can check to see what kind of message it is.
Joeatsumi 0:d22fc86a8387 190
Joeatsumi 0:d22fc86a8387 191 if ( compareMsgHeader(NAV_VELNED_HEADER) ) {
Joeatsumi 0:d22fc86a8387 192 currentMsgType = MT_NAV_VELNED;
Joeatsumi 0:d22fc86a8387 193 payloadSize = sizeof(NAV_VELNED);
Joeatsumi 0:d22fc86a8387 194
Joeatsumi 0:d22fc86a8387 195 }
Joeatsumi 0:d22fc86a8387 196 else if ( compareMsgHeader(NAV_STATUS_HEADER) ) {
Joeatsumi 0:d22fc86a8387 197 currentMsgType = MT_NAV_STATUS;
Joeatsumi 0:d22fc86a8387 198 payloadSize = sizeof(NAV_STATUS);
Joeatsumi 0:d22fc86a8387 199 }
Joeatsumi 0:d22fc86a8387 200
Joeatsumi 0:d22fc86a8387 201 else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
Joeatsumi 0:d22fc86a8387 202 currentMsgType = MT_NAV_POSLLH;
Joeatsumi 0:d22fc86a8387 203 payloadSize = sizeof(NAV_POSLLH);
Joeatsumi 0:d22fc86a8387 204
Joeatsumi 0:d22fc86a8387 205 }
Joeatsumi 0:d22fc86a8387 206
Joeatsumi 0:d22fc86a8387 207 else {
Joeatsumi 0:d22fc86a8387 208 // unknown message type, bail
Joeatsumi 0:d22fc86a8387 209 fpos = 0;
Joeatsumi 0:d22fc86a8387 210 continue;
Joeatsumi 0:d22fc86a8387 211 }
Joeatsumi 0:d22fc86a8387 212 }
Joeatsumi 0:d22fc86a8387 213
Joeatsumi 0:d22fc86a8387 214 if ( fpos == (payloadSize+2) ) {
Joeatsumi 0:d22fc86a8387 215 // All payload bytes have now been received, so we can calculate the
Joeatsumi 0:d22fc86a8387 216 // expected checksum value to compare with the next two incoming bytes.
Joeatsumi 0:d22fc86a8387 217 calcChecksum(checksum, payloadSize);
Joeatsumi 0:d22fc86a8387 218 }
Joeatsumi 0:d22fc86a8387 219 else if ( fpos == (payloadSize+3) ) {
Joeatsumi 0:d22fc86a8387 220 // First byte after the payload, ie. first byte of the checksum.
Joeatsumi 0:d22fc86a8387 221 // Does it match the first byte of the checksum we calculated?
Joeatsumi 0:d22fc86a8387 222 if ( c != checksum[0] ) {
Joeatsumi 0:d22fc86a8387 223 // Checksum doesn't match, reset to beginning state and try again.
Joeatsumi 0:d22fc86a8387 224 fpos = 0;
Joeatsumi 0:d22fc86a8387 225 }
Joeatsumi 0:d22fc86a8387 226 }
Joeatsumi 0:d22fc86a8387 227 else if ( fpos == (payloadSize+4) ) {
Joeatsumi 0:d22fc86a8387 228 // Second byte after the payload, ie. second byte of the checksum.
Joeatsumi 0:d22fc86a8387 229 // Does it match the second byte of the checksum we calculated?
Joeatsumi 0:d22fc86a8387 230 fpos = 0; // We will reset the state regardless of whether the checksum matches.
Joeatsumi 0:d22fc86a8387 231 if ( c == checksum[1] ) {
Joeatsumi 0:d22fc86a8387 232 // Checksum matches, we have a valid message.
Joeatsumi 0:d22fc86a8387 233 if(currentMsgType==MT_NAV_POSLLH){
Joeatsumi 0:d22fc86a8387 234 latitude=ubxMessage.navPosllh.lat/10000000.0f;
Joeatsumi 0:d22fc86a8387 235 longitude=ubxMessage.navPosllh.lon/10000000.0f;
Joeatsumi 0:d22fc86a8387 236 height_float=float(ubxMessage.navPosllh.height);
Joeatsumi 0:d22fc86a8387 237
Joeatsumi 0:d22fc86a8387 238 flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる
Joeatsumi 0:d22fc86a8387 239 }
Joeatsumi 0:d22fc86a8387 240 else if(currentMsgType==MT_NAV_VELNED){
Joeatsumi 0:d22fc86a8387 241 velN_float=float(ubxMessage.navVelned.velN);
Joeatsumi 0:d22fc86a8387 242 velE_float=float(ubxMessage.navVelned.velE);
Joeatsumi 0:d22fc86a8387 243 velD_float=float(ubxMessage.navVelned.velD);
Joeatsumi 0:d22fc86a8387 244
Joeatsumi 0:d22fc86a8387 245 flag_velned=1;//速度情報を読み取った合図としてフラグを立てる
Joeatsumi 0:d22fc86a8387 246
Joeatsumi 0:d22fc86a8387 247 }
Joeatsumi 0:d22fc86a8387 248 else if(currentMsgType==MT_NAV_STATUS){
Joeatsumi 0:d22fc86a8387 249
Joeatsumi 0:d22fc86a8387 250 }
Joeatsumi 0:d22fc86a8387 251
Joeatsumi 0:d22fc86a8387 252 //return currentMsgType;
Joeatsumi 0:d22fc86a8387 253 }
Joeatsumi 0:d22fc86a8387 254 }
Joeatsumi 0:d22fc86a8387 255 else if ( fpos > (payloadSize+4) ) {
Joeatsumi 0:d22fc86a8387 256 // We have now read more bytes than both the expected payload and checksum
Joeatsumi 0:d22fc86a8387 257 // together, so something went wrong. Reset to beginning state and try again.
Joeatsumi 0:d22fc86a8387 258 fpos = 0;
Joeatsumi 0:d22fc86a8387 259 }
Joeatsumi 0:d22fc86a8387 260 }
Joeatsumi 0:d22fc86a8387 261 }
Joeatsumi 0:d22fc86a8387 262
Joeatsumi 0:d22fc86a8387 263 CS = 1; //SPIによる読み出しを終了させる
Joeatsumi 0:d22fc86a8387 264
Joeatsumi 0:d22fc86a8387 265 /*
Joeatsumi 0:d22fc86a8387 266 processGPS()の処理に必要な時間の計測
Joeatsumi 0:d22fc86a8387 267 複数のメッセージを読み取る、つまりこの関数をメッセージの数だけwhile内で読み出すとき、
Joeatsumi 0:d22fc86a8387 268 この関数の処理時間(processed_time)として保存されるのは
Joeatsumi 0:d22fc86a8387 269 最後に呼び出されたprocessGPSの処理時間となる。
Joeatsumi 0:d22fc86a8387 270 */
Joeatsumi 0:d22fc86a8387 271 processed_time_after = processing_timer.read_us();// captureing prossing time
Joeatsumi 0:d22fc86a8387 272 processed_time=processed_time_after-processed_time_before;
Joeatsumi 0:d22fc86a8387 273
Joeatsumi 0:d22fc86a8387 274 }
Joeatsumi 0:d22fc86a8387 275
Joeatsumi 0:d22fc86a8387 276 /*--------------------------------------------*/
Joeatsumi 0:d22fc86a8387 277 int main() {
Joeatsumi 0:d22fc86a8387 278
Joeatsumi 0:d22fc86a8387 279 //UART initialization
Joeatsumi 0:d22fc86a8387 280 pc.baud(460800); //115.2 kbps
Joeatsumi 0:d22fc86a8387 281
Joeatsumi 0:d22fc86a8387 282 //フラグのリセット
Joeatsumi 0:d22fc86a8387 283 flag_posllh=0;
Joeatsumi 0:d22fc86a8387 284 flag_velned=0;
Joeatsumi 0:d22fc86a8387 285
Joeatsumi 0:d22fc86a8387 286 processing_timer.start();//timer starts
Joeatsumi 0:d22fc86a8387 287
Joeatsumi 0:d22fc86a8387 288 while(1) {
Joeatsumi 0:d22fc86a8387 289
Joeatsumi 0:d22fc86a8387 290 /*3種類のメッセージがNEO-7Mから来るので、processGPS()は3回読み出す
Joeatsumi 0:d22fc86a8387 291 送信されるメッセージの数だけprocessGPS()を呼び出せばよい。
Joeatsumi 0:d22fc86a8387 292 */
Joeatsumi 0:d22fc86a8387 293 processGPS();
Joeatsumi 0:d22fc86a8387 294 processGPS();
Joeatsumi 0:d22fc86a8387 295 processGPS();
Joeatsumi 0:d22fc86a8387 296
Joeatsumi 0:d22fc86a8387 297 if((flag_posllh==1)&&(flag_velned==1)){//位置と速度情報を読み取ったら場合
Joeatsumi 0:d22fc86a8387 298
Joeatsumi 0:d22fc86a8387 299 /*Teratermでロギングする用の表示*/
Joeatsumi 0:d22fc86a8387 300 pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float);
Joeatsumi 0:d22fc86a8387 301 /*計測ではなくデバッグ用の表示*/
Joeatsumi 0:d22fc86a8387 302 //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
Joeatsumi 0:d22fc86a8387 303 //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
Joeatsumi 0:d22fc86a8387 304 //pc.printf("processed_time(us)=%d\r\n",processed_time);
Joeatsumi 0:d22fc86a8387 305
Joeatsumi 0:d22fc86a8387 306 /*processGPSの処理時間の表示*/
Joeatsumi 0:d22fc86a8387 307 //pc.printf("processed_time_before(us)=%d\r\n",(processed_time_before));
Joeatsumi 0:d22fc86a8387 308 //pc.printf("processed_time_after(us)=%d\r\n",(processed_time_after));
Joeatsumi 0:d22fc86a8387 309
Joeatsumi 0:d22fc86a8387 310 /*フラグを0に戻す*/
Joeatsumi 0:d22fc86a8387 311 flag_posllh=0;
Joeatsumi 0:d22fc86a8387 312 flag_velned=0;
Joeatsumi 0:d22fc86a8387 313
Joeatsumi 0:d22fc86a8387 314 }
Joeatsumi 0:d22fc86a8387 315 else{}
Joeatsumi 0:d22fc86a8387 316
Joeatsumi 0:d22fc86a8387 317 /*
Joeatsumi 0:d22fc86a8387 318 while文の最後にあるwaitは、NEO7-Mを5Hzで測位させているのに合わせて設定している。
Joeatsumi 0:d22fc86a8387 319 他のセンサの計測値を高頻度で取得させる場合、u-bloxモジュールからの情報の処理は
Joeatsumi 0:d22fc86a8387 320 while文ではなく定期的な割り込み処理でもいいかもしれない。
Joeatsumi 0:d22fc86a8387 321 */
Joeatsumi 0:d22fc86a8387 322 wait(0.2);
Joeatsumi 0:d22fc86a8387 323
Joeatsumi 0:d22fc86a8387 324 }//while
Joeatsumi 0:d22fc86a8387 325
Joeatsumi 0:d22fc86a8387 326 }