UBX decode program that is modified from i2dforce UBX program for NAV-STATUS and NAV POSLLH. I used NEO-7M and mbed LPC1768.
main.cpp@1:63a0fd33d5a5, 2020-12-21 (annotated)
- Committer:
- Joeatsumi
- Date:
- Mon Dec 21 16:49:18 2020 +0000
- Revision:
- 1:63a0fd33d5a5
- Parent:
- 0:a9095d98a2b9
- Child:
- 2:2b6b55edb50c
modified ver2 2020/12/22
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Joeatsumi | 1:63a0fd33d5a5 | 1 | /* |
Joeatsumi | 1:63a0fd33d5a5 | 2 | このプログラムはNEO-7MからUBX形式でUARTを介して位置、速度(NAV-VELNED)、測位状態(NAV-STATUS)を読み取るプログラムである。 |
Joeatsumi | 1:63a0fd33d5a5 | 3 | このプログラムには欠点があり、NAV-POSSLHとNAV-VELNEDを同時に読み取る事が出来ない点である。 |
Joeatsumi | 1:63a0fd33d5a5 | 4 | 具体的には。1Hzでその3つの情報を |
Joeatsumi | 1:63a0fd33d5a5 | 5 | */ |
Joeatsumi | 0:a9095d98a2b9 | 6 | |
Joeatsumi | 1:63a0fd33d5a5 | 7 | #include "mbed.h" |
Joeatsumi | 0:a9095d98a2b9 | 8 | |
Joeatsumi | 0:a9095d98a2b9 | 9 | DigitalOut myled(LED1); |
Joeatsumi | 0:a9095d98a2b9 | 10 | |
Joeatsumi | 0:a9095d98a2b9 | 11 | // serial port |
Joeatsumi | 0:a9095d98a2b9 | 12 | Serial pc(USBTX, USBRX); // tx, rx |
Joeatsumi | 1:63a0fd33d5a5 | 13 | Serial ublox_ubx(p9,p10);//tx,rx |
Joeatsumi | 0:a9095d98a2b9 | 14 | |
Joeatsumi | 1:63a0fd33d5a5 | 15 | //variables for decodeing |
Joeatsumi | 1:63a0fd33d5a5 | 16 | float latitude,longitude,height_float; |
Joeatsumi | 1:63a0fd33d5a5 | 17 | int gps_Fix; |
Joeatsumi | 1:63a0fd33d5a5 | 18 | float velN_float,velE_float,velD_float; |
Joeatsumi | 1:63a0fd33d5a5 | 19 | |
Joeatsumi | 1:63a0fd33d5a5 | 20 | int flag_posllh,flag_velned; |
Joeatsumi | 1:63a0fd33d5a5 | 21 | // |
Joeatsumi | 1:63a0fd33d5a5 | 22 | |
Joeatsumi | 1:63a0fd33d5a5 | 23 | //Header char |
Joeatsumi | 0:a9095d98a2b9 | 24 | const unsigned char UBX_HEADER[] = { 0xB5, 0x62 }; |
Joeatsumi | 0:a9095d98a2b9 | 25 | const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 }; |
Joeatsumi | 0:a9095d98a2b9 | 26 | const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 }; |
Joeatsumi | 0:a9095d98a2b9 | 27 | |
Joeatsumi | 1:63a0fd33d5a5 | 28 | const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 }; |
Joeatsumi | 1:63a0fd33d5a5 | 29 | |
Joeatsumi | 1:63a0fd33d5a5 | 30 | // |
Joeatsumi | 0:a9095d98a2b9 | 31 | enum _ubxMsgType { |
Joeatsumi | 0:a9095d98a2b9 | 32 | MT_NONE, |
Joeatsumi | 0:a9095d98a2b9 | 33 | MT_NAV_POSLLH, |
Joeatsumi | 1:63a0fd33d5a5 | 34 | MT_NAV_STATUS, |
Joeatsumi | 1:63a0fd33d5a5 | 35 | MT_NAV_VELNED |
Joeatsumi | 0:a9095d98a2b9 | 36 | }; |
Joeatsumi | 0:a9095d98a2b9 | 37 | |
Joeatsumi | 0:a9095d98a2b9 | 38 | struct NAV_POSLLH { |
Joeatsumi | 0:a9095d98a2b9 | 39 | unsigned char cls; |
Joeatsumi | 0:a9095d98a2b9 | 40 | unsigned char id; |
Joeatsumi | 0:a9095d98a2b9 | 41 | unsigned short len; |
Joeatsumi | 0:a9095d98a2b9 | 42 | unsigned long iTOW; |
Joeatsumi | 0:a9095d98a2b9 | 43 | long lon; |
Joeatsumi | 0:a9095d98a2b9 | 44 | long lat; |
Joeatsumi | 0:a9095d98a2b9 | 45 | long height; |
Joeatsumi | 0:a9095d98a2b9 | 46 | long hMSL; |
Joeatsumi | 0:a9095d98a2b9 | 47 | unsigned long hAcc; |
Joeatsumi | 0:a9095d98a2b9 | 48 | unsigned long vAcc; |
Joeatsumi | 0:a9095d98a2b9 | 49 | }; |
Joeatsumi | 0:a9095d98a2b9 | 50 | |
Joeatsumi | 0:a9095d98a2b9 | 51 | struct NAV_STATUS { |
Joeatsumi | 0:a9095d98a2b9 | 52 | unsigned char cls; |
Joeatsumi | 0:a9095d98a2b9 | 53 | unsigned char id; |
Joeatsumi | 0:a9095d98a2b9 | 54 | unsigned short len; |
Joeatsumi | 0:a9095d98a2b9 | 55 | unsigned long iTOW; |
Joeatsumi | 0:a9095d98a2b9 | 56 | unsigned char gpsFix; |
Joeatsumi | 0:a9095d98a2b9 | 57 | char flags; |
Joeatsumi | 0:a9095d98a2b9 | 58 | char fixStat; |
Joeatsumi | 0:a9095d98a2b9 | 59 | char flags2; |
Joeatsumi | 0:a9095d98a2b9 | 60 | unsigned long ttff; |
Joeatsumi | 0:a9095d98a2b9 | 61 | unsigned long msss; |
Joeatsumi | 0:a9095d98a2b9 | 62 | }; |
Joeatsumi | 0:a9095d98a2b9 | 63 | |
Joeatsumi | 1:63a0fd33d5a5 | 64 | struct NAV_VELNED { |
Joeatsumi | 1:63a0fd33d5a5 | 65 | unsigned char cls; |
Joeatsumi | 1:63a0fd33d5a5 | 66 | unsigned char id; |
Joeatsumi | 1:63a0fd33d5a5 | 67 | unsigned short len; |
Joeatsumi | 1:63a0fd33d5a5 | 68 | unsigned long iTOW; |
Joeatsumi | 1:63a0fd33d5a5 | 69 | signed long velN; |
Joeatsumi | 1:63a0fd33d5a5 | 70 | signed long velE; |
Joeatsumi | 1:63a0fd33d5a5 | 71 | signed long velD; |
Joeatsumi | 1:63a0fd33d5a5 | 72 | unsigned long speed; |
Joeatsumi | 1:63a0fd33d5a5 | 73 | unsigned long gSpeed; |
Joeatsumi | 1:63a0fd33d5a5 | 74 | signed long heading; |
Joeatsumi | 1:63a0fd33d5a5 | 75 | unsigned long sAcc; |
Joeatsumi | 1:63a0fd33d5a5 | 76 | unsigned long cAcc; |
Joeatsumi | 1:63a0fd33d5a5 | 77 | |
Joeatsumi | 1:63a0fd33d5a5 | 78 | }; |
Joeatsumi | 1:63a0fd33d5a5 | 79 | |
Joeatsumi | 0:a9095d98a2b9 | 80 | union UBXMessage { |
Joeatsumi | 1:63a0fd33d5a5 | 81 | NAV_VELNED navVelned;//payload size is 36bytes |
Joeatsumi | 1:63a0fd33d5a5 | 82 | NAV_POSLLH navPosllh;//payload size is 28bytes |
Joeatsumi | 1:63a0fd33d5a5 | 83 | NAV_STATUS navStatus;//payload size is 16bytes |
Joeatsumi | 0:a9095d98a2b9 | 84 | }; |
Joeatsumi | 0:a9095d98a2b9 | 85 | |
Joeatsumi | 0:a9095d98a2b9 | 86 | UBXMessage ubxMessage; |
Joeatsumi | 0:a9095d98a2b9 | 87 | |
Joeatsumi | 0:a9095d98a2b9 | 88 | // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid. |
Joeatsumi | 0:a9095d98a2b9 | 89 | // The procedure used to calculate this is given as pseudo-code in the uBlox manual. |
Joeatsumi | 0:a9095d98a2b9 | 90 | void calcChecksum(unsigned char* CK, int msgSize) { |
Joeatsumi | 0:a9095d98a2b9 | 91 | memset(CK, 0, 2); |
Joeatsumi | 0:a9095d98a2b9 | 92 | for (int i = 0; i < msgSize; i++) { |
Joeatsumi | 0:a9095d98a2b9 | 93 | CK[0] += ((unsigned char*)(&ubxMessage))[i]; |
Joeatsumi | 0:a9095d98a2b9 | 94 | CK[1] += CK[0]; |
Joeatsumi | 0:a9095d98a2b9 | 95 | } |
Joeatsumi | 0:a9095d98a2b9 | 96 | } |
Joeatsumi | 0:a9095d98a2b9 | 97 | |
Joeatsumi | 0:a9095d98a2b9 | 98 | |
Joeatsumi | 0:a9095d98a2b9 | 99 | // Compares the first two bytes of the ubxMessage struct with a specific message header. |
Joeatsumi | 0:a9095d98a2b9 | 100 | // Returns true if the two bytes match. |
Joeatsumi | 0:a9095d98a2b9 | 101 | bool compareMsgHeader(const unsigned char* msgHeader) { |
Joeatsumi | 0:a9095d98a2b9 | 102 | unsigned char* ptr = (unsigned char*)(&ubxMessage); |
Joeatsumi | 0:a9095d98a2b9 | 103 | return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1]; |
Joeatsumi | 0:a9095d98a2b9 | 104 | } |
Joeatsumi | 0:a9095d98a2b9 | 105 | |
Joeatsumi | 0:a9095d98a2b9 | 106 | |
Joeatsumi | 0:a9095d98a2b9 | 107 | // Reads in bytes from the GPS module and checks to see if a valid message has been constructed. |
Joeatsumi | 0:a9095d98a2b9 | 108 | // Returns the type of the message found if successful, or MT_NONE if no message was found. |
Joeatsumi | 0:a9095d98a2b9 | 109 | // After a successful return the contents of the ubxMessage union will be valid, for the |
Joeatsumi | 0:a9095d98a2b9 | 110 | // message type that was found. Note that further calls to this function can invalidate the |
Joeatsumi | 0:a9095d98a2b9 | 111 | // message content, so you must use the obtained values before calling this function again. |
Joeatsumi | 0:a9095d98a2b9 | 112 | int processGPS() { |
Joeatsumi | 1:63a0fd33d5a5 | 113 | |
Joeatsumi | 0:a9095d98a2b9 | 114 | static int fpos = 0; |
Joeatsumi | 0:a9095d98a2b9 | 115 | static unsigned char checksum[2]; |
Joeatsumi | 0:a9095d98a2b9 | 116 | |
Joeatsumi | 0:a9095d98a2b9 | 117 | static unsigned char currentMsgType = MT_NONE; |
Joeatsumi | 0:a9095d98a2b9 | 118 | static int payloadSize = sizeof(UBXMessage); |
Joeatsumi | 0:a9095d98a2b9 | 119 | |
Joeatsumi | 0:a9095d98a2b9 | 120 | while ( ublox_ubx.readable() ) { |
Joeatsumi | 0:a9095d98a2b9 | 121 | |
Joeatsumi | 0:a9095d98a2b9 | 122 | unsigned char c = ublox_ubx.getc(); |
Joeatsumi | 0:a9095d98a2b9 | 123 | //Serial.write(c); |
Joeatsumi | 0:a9095d98a2b9 | 124 | |
Joeatsumi | 0:a9095d98a2b9 | 125 | if ( fpos < 2 ) { |
Joeatsumi | 0:a9095d98a2b9 | 126 | // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62) |
Joeatsumi | 0:a9095d98a2b9 | 127 | if ( c == UBX_HEADER[fpos] ) |
Joeatsumi | 0:a9095d98a2b9 | 128 | fpos++; |
Joeatsumi | 0:a9095d98a2b9 | 129 | else |
Joeatsumi | 0:a9095d98a2b9 | 130 | fpos = 0; // Reset to beginning state. |
Joeatsumi | 0:a9095d98a2b9 | 131 | } |
Joeatsumi | 0:a9095d98a2b9 | 132 | else { |
Joeatsumi | 0:a9095d98a2b9 | 133 | // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER |
Joeatsumi | 0:a9095d98a2b9 | 134 | // and we are now reading in the bytes that make up the payload. |
Joeatsumi | 0:a9095d98a2b9 | 135 | |
Joeatsumi | 0:a9095d98a2b9 | 136 | // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because |
Joeatsumi | 0:a9095d98a2b9 | 137 | // the struct does not include the initial two-byte header (UBX_HEADER). |
Joeatsumi | 0:a9095d98a2b9 | 138 | if ( (fpos-2) < payloadSize ) |
Joeatsumi | 0:a9095d98a2b9 | 139 | ((unsigned char*)(&ubxMessage))[fpos-2] = c; |
Joeatsumi | 0:a9095d98a2b9 | 140 | |
Joeatsumi | 0:a9095d98a2b9 | 141 | fpos++; |
Joeatsumi | 0:a9095d98a2b9 | 142 | |
Joeatsumi | 0:a9095d98a2b9 | 143 | if ( fpos == 4 ) { |
Joeatsumi | 0:a9095d98a2b9 | 144 | // We have just received the second byte of the message type header, |
Joeatsumi | 0:a9095d98a2b9 | 145 | // so now we can check to see what kind of message it is. |
Joeatsumi | 1:63a0fd33d5a5 | 146 | |
Joeatsumi | 1:63a0fd33d5a5 | 147 | if ( compareMsgHeader(NAV_VELNED_HEADER) ) { |
Joeatsumi | 1:63a0fd33d5a5 | 148 | currentMsgType = MT_NAV_VELNED; |
Joeatsumi | 1:63a0fd33d5a5 | 149 | payloadSize = sizeof(NAV_VELNED); |
Joeatsumi | 1:63a0fd33d5a5 | 150 | |
Joeatsumi | 0:a9095d98a2b9 | 151 | } |
Joeatsumi | 0:a9095d98a2b9 | 152 | else if ( compareMsgHeader(NAV_STATUS_HEADER) ) { |
Joeatsumi | 0:a9095d98a2b9 | 153 | currentMsgType = MT_NAV_STATUS; |
Joeatsumi | 0:a9095d98a2b9 | 154 | payloadSize = sizeof(NAV_STATUS); |
Joeatsumi | 0:a9095d98a2b9 | 155 | } |
Joeatsumi | 1:63a0fd33d5a5 | 156 | |
Joeatsumi | 1:63a0fd33d5a5 | 157 | else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) { |
Joeatsumi | 1:63a0fd33d5a5 | 158 | currentMsgType = MT_NAV_POSLLH; |
Joeatsumi | 1:63a0fd33d5a5 | 159 | payloadSize = sizeof(NAV_POSLLH); |
Joeatsumi | 1:63a0fd33d5a5 | 160 | |
Joeatsumi | 1:63a0fd33d5a5 | 161 | } |
Joeatsumi | 1:63a0fd33d5a5 | 162 | |
Joeatsumi | 0:a9095d98a2b9 | 163 | else { |
Joeatsumi | 0:a9095d98a2b9 | 164 | // unknown message type, bail |
Joeatsumi | 0:a9095d98a2b9 | 165 | fpos = 0; |
Joeatsumi | 0:a9095d98a2b9 | 166 | continue; |
Joeatsumi | 0:a9095d98a2b9 | 167 | } |
Joeatsumi | 0:a9095d98a2b9 | 168 | } |
Joeatsumi | 0:a9095d98a2b9 | 169 | |
Joeatsumi | 0:a9095d98a2b9 | 170 | if ( fpos == (payloadSize+2) ) { |
Joeatsumi | 0:a9095d98a2b9 | 171 | // All payload bytes have now been received, so we can calculate the |
Joeatsumi | 0:a9095d98a2b9 | 172 | // expected checksum value to compare with the next two incoming bytes. |
Joeatsumi | 0:a9095d98a2b9 | 173 | calcChecksum(checksum, payloadSize); |
Joeatsumi | 0:a9095d98a2b9 | 174 | } |
Joeatsumi | 0:a9095d98a2b9 | 175 | else if ( fpos == (payloadSize+3) ) { |
Joeatsumi | 0:a9095d98a2b9 | 176 | // First byte after the payload, ie. first byte of the checksum. |
Joeatsumi | 0:a9095d98a2b9 | 177 | // Does it match the first byte of the checksum we calculated? |
Joeatsumi | 0:a9095d98a2b9 | 178 | if ( c != checksum[0] ) { |
Joeatsumi | 0:a9095d98a2b9 | 179 | // Checksum doesn't match, reset to beginning state and try again. |
Joeatsumi | 0:a9095d98a2b9 | 180 | fpos = 0; |
Joeatsumi | 0:a9095d98a2b9 | 181 | } |
Joeatsumi | 0:a9095d98a2b9 | 182 | } |
Joeatsumi | 0:a9095d98a2b9 | 183 | else if ( fpos == (payloadSize+4) ) { |
Joeatsumi | 0:a9095d98a2b9 | 184 | // Second byte after the payload, ie. second byte of the checksum. |
Joeatsumi | 0:a9095d98a2b9 | 185 | // Does it match the second byte of the checksum we calculated? |
Joeatsumi | 0:a9095d98a2b9 | 186 | fpos = 0; // We will reset the state regardless of whether the checksum matches. |
Joeatsumi | 0:a9095d98a2b9 | 187 | if ( c == checksum[1] ) { |
Joeatsumi | 0:a9095d98a2b9 | 188 | // Checksum matches, we have a valid message. |
Joeatsumi | 1:63a0fd33d5a5 | 189 | if(currentMsgType==MT_NAV_POSLLH){ |
Joeatsumi | 1:63a0fd33d5a5 | 190 | latitude=ubxMessage.navPosllh.lat/10000000.0f; |
Joeatsumi | 1:63a0fd33d5a5 | 191 | longitude=ubxMessage.navPosllh.lon/10000000.0f; |
Joeatsumi | 1:63a0fd33d5a5 | 192 | height_float=float(ubxMessage.navPosllh.height); |
Joeatsumi | 1:63a0fd33d5a5 | 193 | flag_posllh=1; |
Joeatsumi | 1:63a0fd33d5a5 | 194 | } |
Joeatsumi | 1:63a0fd33d5a5 | 195 | else if(currentMsgType==MT_NAV_VELNED){ |
Joeatsumi | 1:63a0fd33d5a5 | 196 | velN_float=float(ubxMessage.navVelned.velN); |
Joeatsumi | 1:63a0fd33d5a5 | 197 | velE_float=float(ubxMessage.navVelned.velE); |
Joeatsumi | 1:63a0fd33d5a5 | 198 | velD_float=float(ubxMessage.navVelned.velD); |
Joeatsumi | 1:63a0fd33d5a5 | 199 | flag_velned=1; |
Joeatsumi | 1:63a0fd33d5a5 | 200 | |
Joeatsumi | 1:63a0fd33d5a5 | 201 | } |
Joeatsumi | 1:63a0fd33d5a5 | 202 | else if(currentMsgType==MT_NAV_STATUS){ |
Joeatsumi | 1:63a0fd33d5a5 | 203 | |
Joeatsumi | 1:63a0fd33d5a5 | 204 | } |
Joeatsumi | 1:63a0fd33d5a5 | 205 | |
Joeatsumi | 0:a9095d98a2b9 | 206 | return currentMsgType; |
Joeatsumi | 0:a9095d98a2b9 | 207 | } |
Joeatsumi | 0:a9095d98a2b9 | 208 | } |
Joeatsumi | 0:a9095d98a2b9 | 209 | else if ( fpos > (payloadSize+4) ) { |
Joeatsumi | 0:a9095d98a2b9 | 210 | // We have now read more bytes than both the expected payload and checksum |
Joeatsumi | 0:a9095d98a2b9 | 211 | // together, so something went wrong. Reset to beginning state and try again. |
Joeatsumi | 0:a9095d98a2b9 | 212 | fpos = 0; |
Joeatsumi | 0:a9095d98a2b9 | 213 | } |
Joeatsumi | 0:a9095d98a2b9 | 214 | } |
Joeatsumi | 0:a9095d98a2b9 | 215 | } |
Joeatsumi | 0:a9095d98a2b9 | 216 | return MT_NONE; |
Joeatsumi | 0:a9095d98a2b9 | 217 | } |
Joeatsumi | 0:a9095d98a2b9 | 218 | |
Joeatsumi | 0:a9095d98a2b9 | 219 | /*--------------------------------------------*/ |
Joeatsumi | 0:a9095d98a2b9 | 220 | int main() { |
Joeatsumi | 0:a9095d98a2b9 | 221 | |
Joeatsumi | 0:a9095d98a2b9 | 222 | //UART initialization |
Joeatsumi | 1:63a0fd33d5a5 | 223 | pc.baud(115200); //115.2 kbps |
Joeatsumi | 1:63a0fd33d5a5 | 224 | ublox_ubx.baud(115200); //115.2 kbps |
Joeatsumi | 1:63a0fd33d5a5 | 225 | |
Joeatsumi | 1:63a0fd33d5a5 | 226 | flag_posllh=0; |
Joeatsumi | 1:63a0fd33d5a5 | 227 | flag_velned=0; |
Joeatsumi | 0:a9095d98a2b9 | 228 | |
Joeatsumi | 0:a9095d98a2b9 | 229 | while(1) { |
Joeatsumi | 0:a9095d98a2b9 | 230 | int msgType = processGPS(); |
Joeatsumi | 1:63a0fd33d5a5 | 231 | |
Joeatsumi | 1:63a0fd33d5a5 | 232 | if((flag_posllh==1)&&(flag_velned==1)){ |
Joeatsumi | 1:63a0fd33d5a5 | 233 | pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); |
Joeatsumi | 1:63a0fd33d5a5 | 234 | pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float); |
Joeatsumi | 1:63a0fd33d5a5 | 235 | |
Joeatsumi | 1:63a0fd33d5a5 | 236 | flag_posllh=0; |
Joeatsumi | 1:63a0fd33d5a5 | 237 | flag_velned=0; |
Joeatsumi | 1:63a0fd33d5a5 | 238 | } |
Joeatsumi | 1:63a0fd33d5a5 | 239 | else{} |
Joeatsumi | 1:63a0fd33d5a5 | 240 | /* |
Joeatsumi | 0:a9095d98a2b9 | 241 | if ( msgType == MT_NAV_POSLLH ) { |
Joeatsumi | 0:a9095d98a2b9 | 242 | latitude=ubxMessage.navPosllh.lat/10000000.0f; |
Joeatsumi | 0:a9095d98a2b9 | 243 | longitude=ubxMessage.navPosllh.lon/10000000.0f; |
Joeatsumi | 0:a9095d98a2b9 | 244 | height_float=float(ubxMessage.navPosllh.height); |
Joeatsumi | 0:a9095d98a2b9 | 245 | |
Joeatsumi | 0:a9095d98a2b9 | 246 | pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); |
Joeatsumi | 0:a9095d98a2b9 | 247 | } |
Joeatsumi | 0:a9095d98a2b9 | 248 | else if ( msgType == MT_NAV_STATUS ) { |
Joeatsumi | 0:a9095d98a2b9 | 249 | gps_Fix=int(ubxMessage.navStatus.gpsFix); |
Joeatsumi | 0:a9095d98a2b9 | 250 | pc.printf("gps_Fix=%d\r\n",gps_Fix); |
Joeatsumi | 0:a9095d98a2b9 | 251 | } |
Joeatsumi | 1:63a0fd33d5a5 | 252 | else if ( msgType == MT_NAV_VELNED ) { |
Joeatsumi | 1:63a0fd33d5a5 | 253 | velN_float=float(ubxMessage.navVelned.velN); |
Joeatsumi | 1:63a0fd33d5a5 | 254 | velE_float=float(ubxMessage.navVelned.velE); |
Joeatsumi | 1:63a0fd33d5a5 | 255 | velD_float=float(ubxMessage.navVelned.velD); |
Joeatsumi | 1:63a0fd33d5a5 | 256 | //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float); |
Joeatsumi | 1:63a0fd33d5a5 | 257 | //Tera termでNED座標系での速度履歴を保存したい場合、以下のコメント出力を用いる。 |
Joeatsumi | 1:63a0fd33d5a5 | 258 | pc.printf("%f,%f,%f\r\n",velN_float,velE_float,velD_float); |
Joeatsumi | 1:63a0fd33d5a5 | 259 | } |
Joeatsumi | 1:63a0fd33d5a5 | 260 | */ |
Joeatsumi | 1:63a0fd33d5a5 | 261 | |
Joeatsumi | 1:63a0fd33d5a5 | 262 | }//while |
Joeatsumi | 0:a9095d98a2b9 | 263 | |
Joeatsumi | 0:a9095d98a2b9 | 264 | } |