Atsumi Toda / Mbed 2 deprecated UBX_GPS_muliti

UBX decode program that is modified from i2dforce UBX program for NAV-STATUS and NAV POSLLH. I used NEO-7M and mbed LPC1768.

Dependencies:   mbed

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?

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