u-blox / gnss

This class provides an API to communicate with a u-blox GNSS chip. The files here were originally part of the C027_Support library (https://developer.mbed.org/teams/ublox/code/C027_Support/ at revision 138:dafbbf31bf76) but have been separated out, primarily for use on the u-blox C030 board where the cellular interace portion of the C027_Support library will instead be provided through the new mbed Cellular API.

Dependents:   example-ublox-at-cellular-interface-ext example-low-power-sleep example-C030-out-of-box-demo example-C030-out-of-box-demo ... more

gnss.cpp@31:9a1ce433f062, 2019-04-03 (annotated)

Committer:
fahim.alavi@u-blox.com
Date:
Wed Apr 03 16:32:30 2019 +0500
Revision:
31:9a1ce433f062
Parent:
30:de9fcdbc4d06
Child:
33:75163fa7e453
NAVX5 support added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rob.meades@u-blox.com 1:ef70a58a6c98 1 /* mbed Microcontroller Library
rob.meades@u-blox.com 1:ef70a58a6c98 2 * Copyright (c) 2017 u-blox
rob.meades@u-blox.com 1:ef70a58a6c98 3 *
rob.meades@u-blox.com 1:ef70a58a6c98 4 * Licensed under the Apache License, Version 2.0 (the "License");
rob.meades@u-blox.com 1:ef70a58a6c98 5 * you may not use this file except in compliance with the License.
rob.meades@u-blox.com 1:ef70a58a6c98 6 * You may obtain a copy of the License at
rob.meades@u-blox.com 1:ef70a58a6c98 7 *
rob.meades@u-blox.com 1:ef70a58a6c98 8 * http://www.apache.org/licenses/LICENSE-2.0
rob.meades@u-blox.com 1:ef70a58a6c98 9 *
rob.meades@u-blox.com 1:ef70a58a6c98 10 * Unless required by applicable law or agreed to in writing, software
rob.meades@u-blox.com 1:ef70a58a6c98 11 * distributed under the License is distributed on an "AS IS" BASIS,
rob.meades@u-blox.com 1:ef70a58a6c98 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
rob.meades@u-blox.com 1:ef70a58a6c98 13 * See the License for the specific language governing permissions and
rob.meades@u-blox.com 1:ef70a58a6c98 14 * limitations under the License.
rob.meades@u-blox.com 1:ef70a58a6c98 15 */
rob.meades@u-blox.com 1:ef70a58a6c98 16
rob.meades@u-blox.com 1:ef70a58a6c98 17 /**
rob.meades@u-blox.com 1:ef70a58a6c98 18 * @file gnss.cpp
rob.meades@u-blox.com 1:ef70a58a6c98 19 * This file defines a class that communicates with a u-blox GNSS chip.
rob.meades@u-blox.com 1:ef70a58a6c98 20 */
rob.meades@u-blox.com 1:ef70a58a6c98 21
rob.meades@u-blox.com 1:ef70a58a6c98 22 #include "mbed.h"
rob.meades@u-blox.com 1:ef70a58a6c98 23 #include "ctype.h"
rob.meades@u-blox.com 1:ef70a58a6c98 24 #include "gnss.h"
rob.meades@u-blox.com 1:ef70a58a6c98 25
fahim.alavi@u-blox.com 16:cb9861f0f4d8 26 #ifdef UBLOX_WEARABLE_FRAMEWORK
fahim.alavi@u-blox.com 16:cb9861f0f4d8 27 #include "SDCardModel.h"
fahim.alavi@u-blox.com 16:cb9861f0f4d8 28 #else
fahim.alavi@u-blox.com 16:cb9861f0f4d8 29 #define SEND_LOGGING_MESSAGE printf
fahim.alavi@u-blox.com 16:cb9861f0f4d8 30 #endif
fahim.alavi@u-blox.com 16:cb9861f0f4d8 31
rob.meades@u-blox.com 1:ef70a58a6c98 32 GnssParser::GnssParser(void)
rob.meades@u-blox.com 1:ef70a58a6c98 33 {
RobMeades 3:2a1cd49ead85 34 // Create the enable pin but set everything to disabled
RobMeades 2:b10ca4aa2e5e 35 _gnssEnable = NULL;
RobMeades 2:b10ca4aa2e5e 36
rob.meades@u-blox.com 4:82308d600690 37 #ifdef TARGET_UBLOX_C030
rob.meades@u-blox.com 1:ef70a58a6c98 38 _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PushPullNoPull, 0);
RobMeades 5:af4baf3c67f3 39 #else
RobMeades 5:af4baf3c67f3 40 _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PullNone, 1);
RobMeades 2:b10ca4aa2e5e 41 #endif
rob.meades@u-blox.com 1:ef70a58a6c98 42 }
rob.meades@u-blox.com 1:ef70a58a6c98 43
rob.meades@u-blox.com 1:ef70a58a6c98 44 GnssParser::~GnssParser(void)
rob.meades@u-blox.com 1:ef70a58a6c98 45 {
RobMeades 2:b10ca4aa2e5e 46 if (_gnssEnable != NULL) {
RobMeades 2:b10ca4aa2e5e 47 *_gnssEnable = 0;
RobMeades 2:b10ca4aa2e5e 48 delete _gnssEnable;
RobMeades 2:b10ca4aa2e5e 49 }
rob.meades@u-blox.com 1:ef70a58a6c98 50 }
rob.meades@u-blox.com 1:ef70a58a6c98 51
rob.meades@u-blox.com 1:ef70a58a6c98 52 void GnssParser::powerOff(void)
rob.meades@u-blox.com 1:ef70a58a6c98 53 {
rob.meades@u-blox.com 6:56eda66d585b 54 // Set the GNSS into backup mode using the command RMX-LPREQ
rob.meades@u-blox.com 1:ef70a58a6c98 55 struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
rob.meades@u-blox.com 1:ef70a58a6c98 56 sendUbx(0x02, 0x41, &msg, sizeof(msg));
rob.meades@u-blox.com 1:ef70a58a6c98 57 }
rob.meades@u-blox.com 1:ef70a58a6c98 58
bqam 13:10108cd3ad23 59 void GnssParser::cutOffPower(void)
bqam 13:10108cd3ad23 60 {
bqam 13:10108cd3ad23 61 //Disabling PA15 to cut off power supply
bqam 13:10108cd3ad23 62 if (_gnssEnable != NULL)
bqam 13:10108cd3ad23 63 *_gnssEnable = 0;
bqam 13:10108cd3ad23 64 wait_ms(1);
bqam 13:10108cd3ad23 65 }
bqam 13:10108cd3ad23 66
rob.meades@u-blox.com 1:ef70a58a6c98 67 void GnssParser::_powerOn(void)
rob.meades@u-blox.com 1:ef70a58a6c98 68 {
RobMeades 2:b10ca4aa2e5e 69 if (_gnssEnable != NULL) {
RobMeades 2:b10ca4aa2e5e 70 *_gnssEnable = 1;
RobMeades 2:b10ca4aa2e5e 71 }
rob.meades@u-blox.com 1:ef70a58a6c98 72 wait_ms (1);
rob.meades@u-blox.com 1:ef70a58a6c98 73 }
rob.meades@u-blox.com 1:ef70a58a6c98 74
rob.meades@u-blox.com 1:ef70a58a6c98 75 int GnssParser::_getMessage(Pipe<char>* pipe, char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 76 {
rob.meades@u-blox.com 1:ef70a58a6c98 77 int unkn = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 78 int sz = pipe->size();
rob.meades@u-blox.com 1:ef70a58a6c98 79 int fr = pipe->free();
rob.meades@u-blox.com 1:ef70a58a6c98 80 if (len > sz)
rob.meades@u-blox.com 1:ef70a58a6c98 81 len = sz;
rob.meades@u-blox.com 1:ef70a58a6c98 82 while (len > 0)
rob.meades@u-blox.com 1:ef70a58a6c98 83 {
rob.meades@u-blox.com 1:ef70a58a6c98 84 // NMEA protocol
rob.meades@u-blox.com 1:ef70a58a6c98 85 pipe->set(unkn);
rob.meades@u-blox.com 1:ef70a58a6c98 86 int nmea = _parseNmea(pipe,len);
rob.meades@u-blox.com 1:ef70a58a6c98 87 if ((nmea != NOT_FOUND) && (unkn > 0))
rob.meades@u-blox.com 1:ef70a58a6c98 88 return UNKNOWN | pipe->get(buf,unkn);
rob.meades@u-blox.com 1:ef70a58a6c98 89 if (nmea == WAIT && fr)
rob.meades@u-blox.com 1:ef70a58a6c98 90 return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 91 if (nmea > 0)
rob.meades@u-blox.com 1:ef70a58a6c98 92 return NMEA | pipe->get(buf,nmea);
rob.meades@u-blox.com 1:ef70a58a6c98 93 // UBX protocol
rob.meades@u-blox.com 1:ef70a58a6c98 94
rob.meades@u-blox.com 1:ef70a58a6c98 95 pipe->set(unkn);
rob.meades@u-blox.com 1:ef70a58a6c98 96 int ubx = _parseUbx(pipe,len);
rob.meades@u-blox.com 1:ef70a58a6c98 97 if ((ubx != NOT_FOUND) && (unkn > 0))
rob.meades@u-blox.com 1:ef70a58a6c98 98 return UNKNOWN | pipe->get(buf,unkn);
rob.meades@u-blox.com 1:ef70a58a6c98 99 if (ubx == WAIT && fr)
rob.meades@u-blox.com 1:ef70a58a6c98 100 return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 101 if (ubx > 0)
rob.meades@u-blox.com 1:ef70a58a6c98 102 return UBX | pipe->get(buf,ubx);
rob.meades@u-blox.com 1:ef70a58a6c98 103
rob.meades@u-blox.com 1:ef70a58a6c98 104 // UNKNOWN
rob.meades@u-blox.com 1:ef70a58a6c98 105 unkn ++;
rob.meades@u-blox.com 1:ef70a58a6c98 106 len--;
rob.meades@u-blox.com 1:ef70a58a6c98 107 }
rob.meades@u-blox.com 1:ef70a58a6c98 108 if (unkn > 0)
rob.meades@u-blox.com 1:ef70a58a6c98 109 return UNKNOWN | pipe->get(buf,unkn);
rob.meades@u-blox.com 1:ef70a58a6c98 110 return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 111 }
rob.meades@u-blox.com 1:ef70a58a6c98 112
rob.meades@u-blox.com 1:ef70a58a6c98 113 int GnssParser::_parseNmea(Pipe<char>* pipe, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 114 {
rob.meades@u-blox.com 1:ef70a58a6c98 115 int o = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 116 int c = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 117 char ch;
rob.meades@u-blox.com 1:ef70a58a6c98 118 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 119 if ('$' != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 6:56eda66d585b 120 // This needs to be extended by crc checking
rob.meades@u-blox.com 1:ef70a58a6c98 121 for (;;)
rob.meades@u-blox.com 1:ef70a58a6c98 122 {
rob.meades@u-blox.com 1:ef70a58a6c98 123 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 124 ch = pipe->next();
rob.meades@u-blox.com 1:ef70a58a6c98 125 if ('*' == ch) break; // crc delimiter
rob.meades@u-blox.com 1:ef70a58a6c98 126 if (!isprint(ch)) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 127 c ^= ch;
rob.meades@u-blox.com 1:ef70a58a6c98 128 }
rob.meades@u-blox.com 1:ef70a58a6c98 129 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 130 ch = _toHex[(c >> 4) & 0xF]; // high nibble
rob.meades@u-blox.com 1:ef70a58a6c98 131 if (ch != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 132 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 133 ch = _toHex[(c >> 0) & 0xF]; // low nibble
rob.meades@u-blox.com 1:ef70a58a6c98 134 if (ch != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 135 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 136 if ('\r' != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 137 if (++o > len) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 138 if ('\n' != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 139 return o;
rob.meades@u-blox.com 1:ef70a58a6c98 140 }
rob.meades@u-blox.com 1:ef70a58a6c98 141
rob.meades@u-blox.com 1:ef70a58a6c98 142 int GnssParser::_parseUbx(Pipe<char>* pipe, int l)
rob.meades@u-blox.com 1:ef70a58a6c98 143 {
rob.meades@u-blox.com 1:ef70a58a6c98 144 int o = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 145 if (++o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 146 if ('\xB5' != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 147 if (++o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 148 if ('\x62' != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 149 o += 4;
rob.meades@u-blox.com 1:ef70a58a6c98 150 if (o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 151 int i,j,ca,cb;
rob.meades@u-blox.com 1:ef70a58a6c98 152 i = pipe->next(); ca = i; cb = ca; // cls
rob.meades@u-blox.com 1:ef70a58a6c98 153 i = pipe->next(); ca += i; cb += ca; // id
rob.meades@u-blox.com 1:ef70a58a6c98 154 i = pipe->next(); ca += i; cb += ca; // len_lsb
rob.meades@u-blox.com 1:ef70a58a6c98 155 j = pipe->next(); ca += j; cb += ca; // len_msb
rob.meades@u-blox.com 1:ef70a58a6c98 156 j = i + (j << 8);
rob.meades@u-blox.com 1:ef70a58a6c98 157 while (j--)
rob.meades@u-blox.com 1:ef70a58a6c98 158 {
rob.meades@u-blox.com 1:ef70a58a6c98 159 if (++o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 160 i = pipe->next();
rob.meades@u-blox.com 1:ef70a58a6c98 161 ca += i;
rob.meades@u-blox.com 1:ef70a58a6c98 162 cb += ca;
rob.meades@u-blox.com 1:ef70a58a6c98 163 }
rob.meades@u-blox.com 1:ef70a58a6c98 164 ca &= 0xFF; cb &= 0xFF;
rob.meades@u-blox.com 1:ef70a58a6c98 165 if (++o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 166 if (ca != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 167 if (++o > l) return WAIT;
rob.meades@u-blox.com 1:ef70a58a6c98 168 if (cb != pipe->next()) return NOT_FOUND;
rob.meades@u-blox.com 1:ef70a58a6c98 169 return o;
rob.meades@u-blox.com 1:ef70a58a6c98 170 }
rob.meades@u-blox.com 1:ef70a58a6c98 171
rob.meades@u-blox.com 1:ef70a58a6c98 172 int GnssParser::send(const char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 173 {
rob.meades@u-blox.com 1:ef70a58a6c98 174 return _send(buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 175 }
rob.meades@u-blox.com 1:ef70a58a6c98 176
rob.meades@u-blox.com 1:ef70a58a6c98 177 int GnssParser::sendNmea(const char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 178 {
rob.meades@u-blox.com 1:ef70a58a6c98 179 char head[1] = { '$' };
rob.meades@u-blox.com 1:ef70a58a6c98 180 char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
rob.meades@u-blox.com 1:ef70a58a6c98 181 int i;
rob.meades@u-blox.com 1:ef70a58a6c98 182 int crc = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 183 for (i = 0; i < len; i ++)
rob.meades@u-blox.com 1:ef70a58a6c98 184 crc ^= *buf++;
rob.meades@u-blox.com 1:ef70a58a6c98 185 i = _send(head, sizeof(head));
rob.meades@u-blox.com 1:ef70a58a6c98 186 i += _send(buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 187 tail[1] = _toHex[(crc > 4) & 0xF0];
rob.meades@u-blox.com 1:ef70a58a6c98 188 tail[2] = _toHex[(crc > 0) & 0x0F];
rob.meades@u-blox.com 1:ef70a58a6c98 189 i += _send(tail, sizeof(tail));
rob.meades@u-blox.com 1:ef70a58a6c98 190 return i;
rob.meades@u-blox.com 1:ef70a58a6c98 191 }
rob.meades@u-blox.com 1:ef70a58a6c98 192
rob.meades@u-blox.com 1:ef70a58a6c98 193 int GnssParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
rob.meades@u-blox.com 1:ef70a58a6c98 194 {
rob.meades@u-blox.com 1:ef70a58a6c98 195 char head[6] = { 0xB5, 0x62, cls, id, (char) len, (char) (len >> 8)};
rob.meades@u-blox.com 1:ef70a58a6c98 196 char crc[2];
rob.meades@u-blox.com 1:ef70a58a6c98 197 int i;
rob.meades@u-blox.com 1:ef70a58a6c98 198 int ca = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 199 int cb = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 200 for (i = 2; i < 6; i ++)
rob.meades@u-blox.com 1:ef70a58a6c98 201 {
rob.meades@u-blox.com 1:ef70a58a6c98 202 ca += head[i];
rob.meades@u-blox.com 1:ef70a58a6c98 203 cb += ca;
rob.meades@u-blox.com 1:ef70a58a6c98 204 }
rob.meades@u-blox.com 1:ef70a58a6c98 205 for (i = 0; i < len; i ++)
rob.meades@u-blox.com 1:ef70a58a6c98 206 {
rob.meades@u-blox.com 1:ef70a58a6c98 207 ca += ((char*)buf)[i];
rob.meades@u-blox.com 1:ef70a58a6c98 208 cb += ca;
rob.meades@u-blox.com 1:ef70a58a6c98 209 }
rob.meades@u-blox.com 1:ef70a58a6c98 210 i = _send(head, sizeof(head));
rob.meades@u-blox.com 1:ef70a58a6c98 211 i += _send(buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 212 crc[0] = ca & 0xFF;
rob.meades@u-blox.com 1:ef70a58a6c98 213 crc[1] = cb & 0xFF;
rob.meades@u-blox.com 1:ef70a58a6c98 214 i += _send(crc, sizeof(crc));
rob.meades@u-blox.com 1:ef70a58a6c98 215 return i;
rob.meades@u-blox.com 1:ef70a58a6c98 216 }
rob.meades@u-blox.com 1:ef70a58a6c98 217
rob.meades@u-blox.com 1:ef70a58a6c98 218 const char* GnssParser::findNmeaItemPos(int ix, const char* start, const char* end)
rob.meades@u-blox.com 1:ef70a58a6c98 219 {
rob.meades@u-blox.com 6:56eda66d585b 220 // Find the start
rob.meades@u-blox.com 1:ef70a58a6c98 221 for (; (start < end) && (ix > 0); start ++)
rob.meades@u-blox.com 1:ef70a58a6c98 222 {
rob.meades@u-blox.com 1:ef70a58a6c98 223 if (*start == ',')
rob.meades@u-blox.com 1:ef70a58a6c98 224 ix --;
rob.meades@u-blox.com 1:ef70a58a6c98 225 }
rob.meades@u-blox.com 6:56eda66d585b 226 // Found and check bounds
rob.meades@u-blox.com 1:ef70a58a6c98 227 if ((ix == 0) && (start < end) &&
rob.meades@u-blox.com 1:ef70a58a6c98 228 (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
rob.meades@u-blox.com 1:ef70a58a6c98 229 return start;
rob.meades@u-blox.com 1:ef70a58a6c98 230 else
rob.meades@u-blox.com 1:ef70a58a6c98 231 return NULL;
rob.meades@u-blox.com 1:ef70a58a6c98 232 }
rob.meades@u-blox.com 1:ef70a58a6c98 233
rob.meades@u-blox.com 1:ef70a58a6c98 234 bool GnssParser::getNmeaItem(int ix, char* buf, int len, double& val)
rob.meades@u-blox.com 1:ef70a58a6c98 235 {
rob.meades@u-blox.com 1:ef70a58a6c98 236 char* end = &buf[len];
rob.meades@u-blox.com 1:ef70a58a6c98 237 const char* pos = findNmeaItemPos(ix, buf, end);
rob.meades@u-blox.com 6:56eda66d585b 238 // Find the start
rob.meades@u-blox.com 1:ef70a58a6c98 239 if (!pos)
rob.meades@u-blox.com 1:ef70a58a6c98 240 return false;
rob.meades@u-blox.com 1:ef70a58a6c98 241 val = strtod(pos, &end);
rob.meades@u-blox.com 6:56eda66d585b 242 // Restore the last character
rob.meades@u-blox.com 1:ef70a58a6c98 243 return (end > pos);
rob.meades@u-blox.com 1:ef70a58a6c98 244 }
rob.meades@u-blox.com 1:ef70a58a6c98 245
rob.meades@u-blox.com 1:ef70a58a6c98 246 bool GnssParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
rob.meades@u-blox.com 1:ef70a58a6c98 247 {
rob.meades@u-blox.com 1:ef70a58a6c98 248 char* end = &buf[len];
rob.meades@u-blox.com 1:ef70a58a6c98 249 const char* pos = findNmeaItemPos(ix, buf, end);
rob.meades@u-blox.com 6:56eda66d585b 250 // Find the start
rob.meades@u-blox.com 1:ef70a58a6c98 251 if (!pos)
rob.meades@u-blox.com 1:ef70a58a6c98 252 return false;
rob.meades@u-blox.com 1:ef70a58a6c98 253 val = (int)strtol(pos, &end, base);
rob.meades@u-blox.com 1:ef70a58a6c98 254 return (end > pos);
rob.meades@u-blox.com 1:ef70a58a6c98 255 }
rob.meades@u-blox.com 1:ef70a58a6c98 256
rob.meades@u-blox.com 1:ef70a58a6c98 257 bool GnssParser::getNmeaItem(int ix, char* buf, int len, char& val)
rob.meades@u-blox.com 1:ef70a58a6c98 258 {
rob.meades@u-blox.com 1:ef70a58a6c98 259 const char* end = &buf[len];
rob.meades@u-blox.com 1:ef70a58a6c98 260 const char* pos = findNmeaItemPos(ix, buf, end);
rob.meades@u-blox.com 6:56eda66d585b 261 // Find the start
rob.meades@u-blox.com 1:ef70a58a6c98 262 if (!pos)
rob.meades@u-blox.com 1:ef70a58a6c98 263 return false;
rob.meades@u-blox.com 6:56eda66d585b 264 // Skip leading spaces
rob.meades@u-blox.com 1:ef70a58a6c98 265 while ((pos < end) && isspace(*pos))
rob.meades@u-blox.com 1:ef70a58a6c98 266 pos++;
rob.meades@u-blox.com 6:56eda66d585b 267 // Check bound
rob.meades@u-blox.com 1:ef70a58a6c98 268 if ((pos < end) &&
rob.meades@u-blox.com 1:ef70a58a6c98 269 (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
rob.meades@u-blox.com 1:ef70a58a6c98 270 {
rob.meades@u-blox.com 1:ef70a58a6c98 271 val = *pos;
rob.meades@u-blox.com 1:ef70a58a6c98 272 return true;
rob.meades@u-blox.com 1:ef70a58a6c98 273 }
rob.meades@u-blox.com 1:ef70a58a6c98 274 return false;
rob.meades@u-blox.com 1:ef70a58a6c98 275 }
rob.meades@u-blox.com 1:ef70a58a6c98 276
rob.meades@u-blox.com 1:ef70a58a6c98 277 bool GnssParser::getNmeaAngle(int ix, char* buf, int len, double& val)
rob.meades@u-blox.com 1:ef70a58a6c98 278 {
rob.meades@u-blox.com 1:ef70a58a6c98 279 char ch;
rob.meades@u-blox.com 1:ef70a58a6c98 280 if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) &&
rob.meades@u-blox.com 1:ef70a58a6c98 281 ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
rob.meades@u-blox.com 1:ef70a58a6c98 282 {
rob.meades@u-blox.com 1:ef70a58a6c98 283 val *= 0.01;
rob.meades@u-blox.com 1:ef70a58a6c98 284 int i = (int)val;
rob.meades@u-blox.com 1:ef70a58a6c98 285 val = (val - i) / 0.6 + i;
rob.meades@u-blox.com 1:ef70a58a6c98 286 if (ch == 'S' || ch == 'W')
rob.meades@u-blox.com 1:ef70a58a6c98 287 val = -val;
rob.meades@u-blox.com 1:ef70a58a6c98 288 return true;
rob.meades@u-blox.com 1:ef70a58a6c98 289 }
rob.meades@u-blox.com 1:ef70a58a6c98 290 return false;
rob.meades@u-blox.com 1:ef70a58a6c98 291 }
fahim.alavi@u-blox.com 8:720841961804 292
fahim.alavi@u-blox.com 8:720841961804 293 int GnssParser::enable_ubx() {
fahim.alavi@u-blox.com 8:720841961804 294
fahim.alavi@u-blox.com 27:405a5e611635 295 unsigned char ubx_cfg_prt[]={0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,0x00, 0x00};
fahim.alavi@u-blox.com 8:720841961804 296 int conf = RETRY;
fahim.alavi@u-blox.com 8:720841961804 297 int length = 0;
fahim.alavi@u-blox.com 8:720841961804 298
fahim.alavi@u-blox.com 8:720841961804 299 while(conf)
fahim.alavi@u-blox.com 8:720841961804 300 {
fahim.alavi@u-blox.com 8:720841961804 301 length = sendUbx(0x06, 0x00, ubx_cfg_prt, sizeof(ubx_cfg_prt));
fahim.alavi@u-blox.com 8:720841961804 302 if(length >= (int)(sizeof(ubx_cfg_prt) + UBX_FRAME_SIZE))
fahim.alavi@u-blox.com 8:720841961804 303 {
fahim.alavi@u-blox.com 8:720841961804 304 wait(5);
fahim.alavi@u-blox.com 8:720841961804 305 break;
fahim.alavi@u-blox.com 8:720841961804 306 }
fahim.alavi@u-blox.com 8:720841961804 307 else
fahim.alavi@u-blox.com 8:720841961804 308 {
fahim.alavi@u-blox.com 8:720841961804 309 conf = conf - 1;
fahim.alavi@u-blox.com 8:720841961804 310 }
fahim.alavi@u-blox.com 8:720841961804 311 }
Bilal Qamar 12:a04f7eeece23 312 return (conf == 0) ? 0 : 1;
fahim.alavi@u-blox.com 8:720841961804 313 }
fahim.alavi@u-blox.com 8:720841961804 314
fahim.alavi@u-blox.com 8:720841961804 315 eUBX_MESSAGE GnssParser::get_ubx_message(char *buff) {
fahim.alavi@u-blox.com 8:720841961804 316 eUBX_MESSAGE return_value = UNKNOWN_UBX;
fahim.alavi@u-blox.com 8:720841961804 317
fahim.alavi@u-blox.com 8:720841961804 318 if(buff[SYNC_CHAR_INDEX_1] == 0xB5 && buff[SYNC_CHAR_INDEX_2] == 0x62) {
fahim.alavi@u-blox.com 8:720841961804 319
fahim.alavi@u-blox.com 8:720841961804 320 switch (buff[MSG_CLASS_INDEX]) {
fahim.alavi@u-blox.com 8:720841961804 321
fahim.alavi@u-blox.com 8:720841961804 322 case NAV: {
fahim.alavi@u-blox.com 8:720841961804 323 switch (buff[MSG_ID_INDEX]) {
fahim.alavi@u-blox.com 8:720841961804 324
fahim.alavi@u-blox.com 8:720841961804 325 case 0x07: {
fahim.alavi@u-blox.com 8:720841961804 326 return_value = UBX_NAV_PVT;
fahim.alavi@u-blox.com 8:720841961804 327 }
fahim.alavi@u-blox.com 8:720841961804 328 break;
fahim.alavi@u-blox.com 8:720841961804 329 case 0x09: {
fahim.alavi@u-blox.com 8:720841961804 330 return_value = UBX_NAV_ODO;
fahim.alavi@u-blox.com 8:720841961804 331 }
fahim.alavi@u-blox.com 8:720841961804 332 break;
fahim.alavi@u-blox.com 21:f91c0334d017 333 case 0x03: {
fahim.alavi@u-blox.com 29:54fd002f2376 334 return_value = UBX_NAV_STATUS;
fahim.alavi@u-blox.com 29:54fd002f2376 335 }
fahim.alavi@u-blox.com 29:54fd002f2376 336 break;
fahim.alavi@u-blox.com 29:54fd002f2376 337 case 0x35: {
fahim.alavi@u-blox.com 29:54fd002f2376 338 return_value = UBX_NAV_SAT;
fahim.alavi@u-blox.com 21:f91c0334d017 339 }
fahim.alavi@u-blox.com 21:f91c0334d017 340 break;
fahim.alavi@u-blox.com 8:720841961804 341 default:
fahim.alavi@u-blox.com 8:720841961804 342 {
fahim.alavi@u-blox.com 8:720841961804 343 return_value = UNKNOWN_UBX;
fahim.alavi@u-blox.com 8:720841961804 344 }
fahim.alavi@u-blox.com 8:720841961804 345 break;
fahim.alavi@u-blox.com 8:720841961804 346 }
fahim.alavi@u-blox.com 8:720841961804 347 }
fahim.alavi@u-blox.com 8:720841961804 348 break;
fahim.alavi@u-blox.com 8:720841961804 349 case ACK: {
fahim.alavi@u-blox.com 8:720841961804 350 switch (buff[MSG_ID_INDEX]) {
fahim.alavi@u-blox.com 8:720841961804 351 case 0x00: {
fahim.alavi@u-blox.com 8:720841961804 352 return_value = UBX_ACK_NAK;
fahim.alavi@u-blox.com 8:720841961804 353 }
fahim.alavi@u-blox.com 8:720841961804 354 break;
fahim.alavi@u-blox.com 8:720841961804 355 case 0x01: {
fahim.alavi@u-blox.com 8:720841961804 356 return_value = UBX_ACK_ACK;
fahim.alavi@u-blox.com 8:720841961804 357 }
fahim.alavi@u-blox.com 8:720841961804 358 break;
fahim.alavi@u-blox.com 8:720841961804 359 default:
fahim.alavi@u-blox.com 8:720841961804 360 {
fahim.alavi@u-blox.com 8:720841961804 361 return_value = UNKNOWN_UBX;
fahim.alavi@u-blox.com 8:720841961804 362 }
fahim.alavi@u-blox.com 8:720841961804 363 break;
fahim.alavi@u-blox.com 8:720841961804 364 }
fahim.alavi@u-blox.com 8:720841961804 365 }
fahim.alavi@u-blox.com 8:720841961804 366 break;
fahim.alavi@u-blox.com 8:720841961804 367 case LOG: {
fahim.alavi@u-blox.com 8:720841961804 368 switch (buff[MSG_ID_INDEX]) {
fahim.alavi@u-blox.com 8:720841961804 369 case 0x11: {
fahim.alavi@u-blox.com 8:720841961804 370 return_value = UBX_LOG_BATCH;
fahim.alavi@u-blox.com 8:720841961804 371 }
fahim.alavi@u-blox.com 8:720841961804 372 break;
fahim.alavi@u-blox.com 8:720841961804 373 default:
fahim.alavi@u-blox.com 8:720841961804 374 {
fahim.alavi@u-blox.com 8:720841961804 375 return_value = UNKNOWN_UBX;
fahim.alavi@u-blox.com 8:720841961804 376 }
fahim.alavi@u-blox.com 8:720841961804 377 break;
fahim.alavi@u-blox.com 8:720841961804 378 }
fahim.alavi@u-blox.com 8:720841961804 379 }
fahim.alavi@u-blox.com 8:720841961804 380 break;
fahim.alavi@u-blox.com 8:720841961804 381 default:
fahim.alavi@u-blox.com 8:720841961804 382 {
fahim.alavi@u-blox.com 8:720841961804 383 return_value = UNKNOWN_UBX;
fahim.alavi@u-blox.com 8:720841961804 384 }
fahim.alavi@u-blox.com 8:720841961804 385 break;
fahim.alavi@u-blox.com 8:720841961804 386 }
fahim.alavi@u-blox.com 8:720841961804 387 }
fahim.alavi@u-blox.com 8:720841961804 388 return return_value;
fahim.alavi@u-blox.com 8:720841961804 389 }
fahim.alavi@u-blox.com 8:720841961804 390
fahim.alavi@u-blox.com 9:cff83b9f5093 391 tUBX_ACK_ACK GnssParser::decode_ubx_cfg_ack_nak_msg(char *buf) {
fahim.alavi@u-blox.com 9:cff83b9f5093 392 tUBX_ACK_ACK return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 393 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 8:720841961804 394
fahim.alavi@u-blox.com 8:720841961804 395 return_decoded_msg.msg_class = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 396 return_decoded_msg.msg_id = buf[index];
fahim.alavi@u-blox.com 8:720841961804 397
fahim.alavi@u-blox.com 8:720841961804 398 return return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 399 }
fahim.alavi@u-blox.com 8:720841961804 400
fahim.alavi@u-blox.com 8:720841961804 401 tUBX_NAV_ODO GnssParser::decode_ubx_nav_odo_msg(char *buf) {
fahim.alavi@u-blox.com 8:720841961804 402 tUBX_NAV_ODO return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 403 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 8:720841961804 404
fahim.alavi@u-blox.com 8:720841961804 405 return_decoded_msg.version = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 406 index +=3; // 3 bytes are reserved
fahim.alavi@u-blox.com 8:720841961804 407
fahim.alavi@u-blox.com 8:720841961804 408 return_decoded_msg.itow = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 409 return_decoded_msg.itow |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 410 return_decoded_msg.itow |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 411 return_decoded_msg.itow |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 412
fahim.alavi@u-blox.com 8:720841961804 413 return_decoded_msg.distance = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 414 return_decoded_msg.distance |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 415 return_decoded_msg.distance |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 416 return_decoded_msg.distance |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 417
fahim.alavi@u-blox.com 8:720841961804 418 return_decoded_msg.totalDistance = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 419 return_decoded_msg.totalDistance |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 420 return_decoded_msg.totalDistance |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 421 return_decoded_msg.totalDistance |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 422
fahim.alavi@u-blox.com 8:720841961804 423 return_decoded_msg.distanceSTD = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 424 return_decoded_msg.distanceSTD |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 425 return_decoded_msg.distanceSTD |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 426 return_decoded_msg.distanceSTD |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 427
fahim.alavi@u-blox.com 8:720841961804 428 return return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 429 }
fahim.alavi@u-blox.com 8:720841961804 430
fahim.alavi@u-blox.com 8:720841961804 431 tUBX_NAV_PVT GnssParser::decode_ubx_nav_pvt_msg(char *buf) {
fahim.alavi@u-blox.com 8:720841961804 432 tUBX_NAV_PVT return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 433 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 8:720841961804 434
fahim.alavi@u-blox.com 8:720841961804 435 return_decoded_msg.itow = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 436 return_decoded_msg.itow |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 437 return_decoded_msg.itow |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 438 return_decoded_msg.itow |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 439
fahim.alavi@u-blox.com 8:720841961804 440 return_decoded_msg.year = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 441 return_decoded_msg.year |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 442
fahim.alavi@u-blox.com 8:720841961804 443 return_decoded_msg.month = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 444
fahim.alavi@u-blox.com 8:720841961804 445 return_decoded_msg.day = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 446
fahim.alavi@u-blox.com 17:6c226e3e9d61 447 // Go to Fix type
fahim.alavi@u-blox.com 17:6c226e3e9d61 448 index = UBX_PAYLOAD_INDEX + 20;
fahim.alavi@u-blox.com 30:de9fcdbc4d06 449 return_decoded_msg.fixType = buf[index++];
fahim.alavi@u-blox.com 30:de9fcdbc4d06 450 return_decoded_msg.flag1 = buf[index];
fahim.alavi@u-blox.com 17:6c226e3e9d61 451
fahim.alavi@u-blox.com 8:720841961804 452 // Go to lon
fahim.alavi@u-blox.com 8:720841961804 453 index = UBX_PAYLOAD_INDEX + 24;
fahim.alavi@u-blox.com 8:720841961804 454
fahim.alavi@u-blox.com 8:720841961804 455 return_decoded_msg.lon = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 456 return_decoded_msg.lon |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 457 return_decoded_msg.lon |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 458 return_decoded_msg.lon |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 459
fahim.alavi@u-blox.com 8:720841961804 460 return_decoded_msg.lat = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 461 return_decoded_msg.lat |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 462 return_decoded_msg.lat |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 463 return_decoded_msg.lat |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 464
fahim.alavi@u-blox.com 8:720841961804 465 return_decoded_msg.height = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 466 return_decoded_msg.height |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 467 return_decoded_msg.height |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 468 return_decoded_msg.height |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 469
fahim.alavi@u-blox.com 17:6c226e3e9d61 470 // Go to gSpeed
fahim.alavi@u-blox.com 17:6c226e3e9d61 471 index = UBX_PAYLOAD_INDEX + 60;
fahim.alavi@u-blox.com 17:6c226e3e9d61 472 return_decoded_msg.speed = buf[index++];
fahim.alavi@u-blox.com 17:6c226e3e9d61 473 return_decoded_msg.speed |= (buf[index++] << 8);
fahim.alavi@u-blox.com 17:6c226e3e9d61 474 return_decoded_msg.speed |= (buf[index++] << 16);
fahim.alavi@u-blox.com 17:6c226e3e9d61 475 return_decoded_msg.speed |= (buf[index++] << 24);
fahim.alavi@u-blox.com 17:6c226e3e9d61 476
fahim.alavi@u-blox.com 8:720841961804 477 return return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 478 }
fahim.alavi@u-blox.com 8:720841961804 479
fahim.alavi@u-blox.com 8:720841961804 480 tUBX_LOG_BATCH GnssParser::decode_ubx_log_batch_msg(char *buf) {
fahim.alavi@u-blox.com 8:720841961804 481 tUBX_LOG_BATCH return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 482 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 8:720841961804 483
fahim.alavi@u-blox.com 8:720841961804 484 // move index itow
fahim.alavi@u-blox.com 8:720841961804 485 index = UBX_PAYLOAD_INDEX + 4;
fahim.alavi@u-blox.com 8:720841961804 486
fahim.alavi@u-blox.com 8:720841961804 487 return_decoded_msg.itow = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 488 return_decoded_msg.itow |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 489 return_decoded_msg.itow |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 490 return_decoded_msg.itow |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 491
fahim.alavi@u-blox.com 8:720841961804 492 // move index lon
fahim.alavi@u-blox.com 8:720841961804 493 index = UBX_PAYLOAD_INDEX + 24;
fahim.alavi@u-blox.com 8:720841961804 494
fahim.alavi@u-blox.com 8:720841961804 495 return_decoded_msg.lon = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 496 return_decoded_msg.lon |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 497 return_decoded_msg.lon |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 498 return_decoded_msg.lon |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 499
fahim.alavi@u-blox.com 8:720841961804 500 return_decoded_msg.lat = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 501 return_decoded_msg.lat |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 502 return_decoded_msg.lat |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 503 return_decoded_msg.lat |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 504
fahim.alavi@u-blox.com 8:720841961804 505 return_decoded_msg.height = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 506 return_decoded_msg.height |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 507 return_decoded_msg.height |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 508 return_decoded_msg.height |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 509
fahim.alavi@u-blox.com 8:720841961804 510 // move index to distance
fahim.alavi@u-blox.com 8:720841961804 511 index = UBX_PAYLOAD_INDEX + 84;
fahim.alavi@u-blox.com 8:720841961804 512
fahim.alavi@u-blox.com 8:720841961804 513 return_decoded_msg.distance = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 514 return_decoded_msg.distance |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 515 return_decoded_msg.distance |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 516 return_decoded_msg.distance |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 517
fahim.alavi@u-blox.com 8:720841961804 518 return_decoded_msg.totalDistance = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 519 return_decoded_msg.totalDistance |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 520 return_decoded_msg.totalDistance |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 521 return_decoded_msg.totalDistance |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 522
fahim.alavi@u-blox.com 8:720841961804 523 return_decoded_msg.distanceSTD = buf[index++];
fahim.alavi@u-blox.com 8:720841961804 524 return_decoded_msg.distanceSTD |= (buf[index++] << 8);
fahim.alavi@u-blox.com 8:720841961804 525 return_decoded_msg.distanceSTD |= (buf[index++] << 16);
fahim.alavi@u-blox.com 8:720841961804 526 return_decoded_msg.distanceSTD |= (buf[index++] << 24);
fahim.alavi@u-blox.com 8:720841961804 527
fahim.alavi@u-blox.com 8:720841961804 528 return return_decoded_msg;
fahim.alavi@u-blox.com 8:720841961804 529 }
fahim.alavi@u-blox.com 10:bfa9081e1d14 530
fahim.alavi@u-blox.com 21:f91c0334d017 531 tUBX_NAV_STATUS GnssParser::decode_ubx_nav_status_msg(char *buf) {
fahim.alavi@u-blox.com 21:f91c0334d017 532
fahim.alavi@u-blox.com 21:f91c0334d017 533 tUBX_NAV_STATUS return_decoded_msg;
fahim.alavi@u-blox.com 21:f91c0334d017 534 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 21:f91c0334d017 535
fahim.alavi@u-blox.com 21:f91c0334d017 536 return_decoded_msg.itow = buf[index++];
fahim.alavi@u-blox.com 21:f91c0334d017 537 return_decoded_msg.itow |= (buf[index++] << 8);
fahim.alavi@u-blox.com 21:f91c0334d017 538 return_decoded_msg.itow |= (buf[index++] << 16);
fahim.alavi@u-blox.com 21:f91c0334d017 539 return_decoded_msg.itow |= (buf[index++] << 24);
fahim.alavi@u-blox.com 21:f91c0334d017 540
fahim.alavi@u-blox.com 21:f91c0334d017 541 // move index flag
fahim.alavi@u-blox.com 21:f91c0334d017 542 return_decoded_msg.fix = buf[index++];
fahim.alavi@u-blox.com 21:f91c0334d017 543
fahim.alavi@u-blox.com 21:f91c0334d017 544 return_decoded_msg.flags = buf[index++];
fahim.alavi@u-blox.com 21:f91c0334d017 545
fahim.alavi@u-blox.com 21:f91c0334d017 546 // move to ttff
fahim.alavi@u-blox.com 21:f91c0334d017 547 index+=2;
fahim.alavi@u-blox.com 21:f91c0334d017 548
fahim.alavi@u-blox.com 21:f91c0334d017 549 return_decoded_msg.ttff = buf[index++];
fahim.alavi@u-blox.com 21:f91c0334d017 550 return_decoded_msg.ttff |= (buf[index++] << 8);
fahim.alavi@u-blox.com 21:f91c0334d017 551 return_decoded_msg.ttff |= (buf[index++] << 16);
fahim.alavi@u-blox.com 21:f91c0334d017 552 return_decoded_msg.ttff |= (buf[index++] << 24);
fahim.alavi@u-blox.com 21:f91c0334d017 553
fahim.alavi@u-blox.com 21:f91c0334d017 554 return_decoded_msg.msss = buf[index++];
fahim.alavi@u-blox.com 21:f91c0334d017 555 return_decoded_msg.msss |= (buf[index++] << 8);
fahim.alavi@u-blox.com 21:f91c0334d017 556 return_decoded_msg.msss |= (buf[index++] << 16);
fahim.alavi@u-blox.com 21:f91c0334d017 557 return_decoded_msg.msss |= (buf[index++] << 24);
fahim.alavi@u-blox.com 21:f91c0334d017 558
fahim.alavi@u-blox.com 21:f91c0334d017 559 return return_decoded_msg;
fahim.alavi@u-blox.com 21:f91c0334d017 560 }
fahim.alavi@u-blox.com 21:f91c0334d017 561
fahim.alavi@u-blox.com 29:54fd002f2376 562
fahim.alavi@u-blox.com 29:54fd002f2376 563 tUBX_NAV_SAT GnssParser::decode_ubx_nav_sat_msg(char *buf, int length) {
fahim.alavi@u-blox.com 29:54fd002f2376 564 tUBX_NAV_SAT return_decoded_msg;
fahim.alavi@u-blox.com 29:54fd002f2376 565 uint8_t index = UBX_PAYLOAD_INDEX;
fahim.alavi@u-blox.com 29:54fd002f2376 566 uint8_t numberSVs = buf[index + 5];
fahim.alavi@u-blox.com 29:54fd002f2376 567
fahim.alavi@u-blox.com 29:54fd002f2376 568 if(length == (UBX_FRAME_SIZE + 8 + (12*numberSVs))) {
fahim.alavi@u-blox.com 29:54fd002f2376 569 return_decoded_msg.status = true;
fahim.alavi@u-blox.com 29:54fd002f2376 570 }
fahim.alavi@u-blox.com 29:54fd002f2376 571 else {
fahim.alavi@u-blox.com 29:54fd002f2376 572 return_decoded_msg.status = false;
fahim.alavi@u-blox.com 29:54fd002f2376 573 }
fahim.alavi@u-blox.com 29:54fd002f2376 574
fahim.alavi@u-blox.com 29:54fd002f2376 575 return return_decoded_msg;
fahim.alavi@u-blox.com 29:54fd002f2376 576 }
fahim.alavi@u-blox.com 29:54fd002f2376 577
fahim.alavi@u-blox.com 10:bfa9081e1d14 578 int GnssParser::ubx_request_batched_data(bool sendMonFirst) {
fahim.alavi@u-blox.com 10:bfa9081e1d14 579 unsigned char ubx_log_retrieve_batch[]={0x00, 0x00, 0x00, 0x00};
fahim.alavi@u-blox.com 10:bfa9081e1d14 580
fahim.alavi@u-blox.com 10:bfa9081e1d14 581 ubx_log_retrieve_batch[1] = (sendMonFirst == true) ? 0x01 : 0x00;
fahim.alavi@u-blox.com 10:bfa9081e1d14 582
fahim.alavi@u-blox.com 10:bfa9081e1d14 583 int conf = RETRY;
fahim.alavi@u-blox.com 10:bfa9081e1d14 584 while(conf)
fahim.alavi@u-blox.com 10:bfa9081e1d14 585 {
fahim.alavi@u-blox.com 10:bfa9081e1d14 586
fahim.alavi@u-blox.com 10:bfa9081e1d14 587 int length = sendUbx(0x21, 0x10, ubx_log_retrieve_batch, sizeof(ubx_log_retrieve_batch));
fahim.alavi@u-blox.com 10:bfa9081e1d14 588 if(length >= (int)(sizeof(ubx_log_retrieve_batch) + UBX_FRAME_SIZE))
fahim.alavi@u-blox.com 10:bfa9081e1d14 589 {
fahim.alavi@u-blox.com 10:bfa9081e1d14 590 wait(5);
fahim.alavi@u-blox.com 10:bfa9081e1d14 591 break;
fahim.alavi@u-blox.com 10:bfa9081e1d14 592 }
fahim.alavi@u-blox.com 10:bfa9081e1d14 593 else
fahim.alavi@u-blox.com 10:bfa9081e1d14 594 {
fahim.alavi@u-blox.com 10:bfa9081e1d14 595 conf = conf - 1;
fahim.alavi@u-blox.com 10:bfa9081e1d14 596 }
fahim.alavi@u-blox.com 10:bfa9081e1d14 597 }
fahim.alavi@u-blox.com 10:bfa9081e1d14 598 if(conf == 0)
fahim.alavi@u-blox.com 10:bfa9081e1d14 599 {
fahim.alavi@u-blox.com 10:bfa9081e1d14 600 return 1;
fahim.alavi@u-blox.com 10:bfa9081e1d14 601 }
fahim.alavi@u-blox.com 10:bfa9081e1d14 602
fahim.alavi@u-blox.com 10:bfa9081e1d14 603 return 0;
fahim.alavi@u-blox.com 10:bfa9081e1d14 604 }
rob.meades@u-blox.com 1:ef70a58a6c98 605
rob.meades@u-blox.com 1:ef70a58a6c98 606 const char GnssParser::_toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
rob.meades@u-blox.com 1:ef70a58a6c98 607
rob.meades@u-blox.com 1:ef70a58a6c98 608 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 609 // Serial Implementation
rob.meades@u-blox.com 1:ef70a58a6c98 610 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 611
rob.meades@u-blox.com 1:ef70a58a6c98 612 GnssSerial::GnssSerial(PinName tx /*= GNSSTXD */, PinName rx /*= GNSSRXD */, int baudrate /*= GNSSBAUD */,
rob.meades@u-blox.com 4:82308d600690 613 int rxSize /*= 256 */, int txSize /*= 128 */) :
rob.meades@u-blox.com 1:ef70a58a6c98 614 SerialPipe(tx, rx, baudrate, rxSize, txSize)
rob.meades@u-blox.com 1:ef70a58a6c98 615 {
rob.meades@u-blox.com 1:ef70a58a6c98 616 baud(baudrate);
rob.meades@u-blox.com 1:ef70a58a6c98 617 }
rob.meades@u-blox.com 1:ef70a58a6c98 618
rob.meades@u-blox.com 1:ef70a58a6c98 619 GnssSerial::~GnssSerial(void)
rob.meades@u-blox.com 1:ef70a58a6c98 620 {
rob.meades@u-blox.com 1:ef70a58a6c98 621 powerOff();
rob.meades@u-blox.com 1:ef70a58a6c98 622 }
rob.meades@u-blox.com 1:ef70a58a6c98 623
rob.meades@u-blox.com 1:ef70a58a6c98 624 bool GnssSerial::init(PinName pn)
rob.meades@u-blox.com 1:ef70a58a6c98 625 {
rob.meades@u-blox.com 6:56eda66d585b 626 Timer timer;
rob.meades@u-blox.com 6:56eda66d585b 627 int size;
rob.meades@u-blox.com 6:56eda66d585b 628
rob.meades@u-blox.com 4:82308d600690 629 // Unused (kept only for compatibility with the I2C version)
rob.meades@u-blox.com 4:82308d600690 630 (void)pn;
rob.meades@u-blox.com 4:82308d600690 631
rob.meades@u-blox.com 1:ef70a58a6c98 632 // Power up and enable the module
rob.meades@u-blox.com 1:ef70a58a6c98 633 _powerOn();
rob.meades@u-blox.com 1:ef70a58a6c98 634
rob.meades@u-blox.com 6:56eda66d585b 635 // Send a byte to wakup the device again
rob.meades@u-blox.com 1:ef70a58a6c98 636 putc(0xFF);
rob.meades@u-blox.com 6:56eda66d585b 637 // Wait until we get some bytes
rob.meades@u-blox.com 6:56eda66d585b 638 size = _pipeRx.size();
rob.meades@u-blox.com 1:ef70a58a6c98 639 timer.start();
rob.meades@u-blox.com 6:56eda66d585b 640 while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) {
rob.meades@u-blox.com 6:56eda66d585b 641 /* Nothing, just wait */
rob.meades@u-blox.com 6:56eda66d585b 642 }
rob.meades@u-blox.com 6:56eda66d585b 643 timer.stop();
rob.meades@u-blox.com 6:56eda66d585b 644
fahim.alavi@u-blox.com 8:720841961804 645 enable_ubx();
fahim.alavi@u-blox.com 8:720841961804 646
fahim.alavi@u-blox.com 15:105cf3ca48c9 647 wait_ms(1000);
fahim.alavi@u-blox.com 15:105cf3ca48c9 648
fahim.alavi@u-blox.com 27:405a5e611635 649 baud(115200);
fahim.alavi@u-blox.com 15:105cf3ca48c9 650
fahim.alavi@u-blox.com 15:105cf3ca48c9 651 // Send a byte to wakup the device again
fahim.alavi@u-blox.com 15:105cf3ca48c9 652 putc(0xFF);
fahim.alavi@u-blox.com 15:105cf3ca48c9 653 // Wait until we get some bytes
fahim.alavi@u-blox.com 15:105cf3ca48c9 654 size = _pipeRx.size();
fahim.alavi@u-blox.com 15:105cf3ca48c9 655 timer.start();
fahim.alavi@u-blox.com 15:105cf3ca48c9 656 while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) {
fahim.alavi@u-blox.com 15:105cf3ca48c9 657 /* Nothing, just wait */
fahim.alavi@u-blox.com 15:105cf3ca48c9 658 }
fahim.alavi@u-blox.com 15:105cf3ca48c9 659
rob.meades@u-blox.com 1:ef70a58a6c98 660 return (size != _pipeRx.size());
rob.meades@u-blox.com 1:ef70a58a6c98 661 }
rob.meades@u-blox.com 1:ef70a58a6c98 662
rob.meades@u-blox.com 1:ef70a58a6c98 663 int GnssSerial::getMessage(char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 664 {
rob.meades@u-blox.com 1:ef70a58a6c98 665 return _getMessage(&_pipeRx, buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 666 }
rob.meades@u-blox.com 1:ef70a58a6c98 667
rob.meades@u-blox.com 1:ef70a58a6c98 668 int GnssSerial::_send(const void* buf, int len)
fahim.alavi@u-blox.com 16:cb9861f0f4d8 669 {
fahim.alavi@u-blox.com 16:cb9861f0f4d8 670 GET_SDCARD_INSTANCE->write(logging_file_name, (void *)buf, len);
fahim.alavi@u-blox.com 16:cb9861f0f4d8 671
rob.meades@u-blox.com 1:ef70a58a6c98 672 return put((const char*)buf, len, true/*=blocking*/);
rob.meades@u-blox.com 1:ef70a58a6c98 673 }
rob.meades@u-blox.com 1:ef70a58a6c98 674
rob.meades@u-blox.com 1:ef70a58a6c98 675 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 676 // I2C Implementation
rob.meades@u-blox.com 1:ef70a58a6c98 677 // ----------------------------------------------------------------
rob.meades@u-blox.com 1:ef70a58a6c98 678
rob.meades@u-blox.com 1:ef70a58a6c98 679 GnssI2C::GnssI2C(PinName sda /*= NC */, PinName scl /*= NC */,
rob.meades@u-blox.com 1:ef70a58a6c98 680 unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) :
rob.meades@u-blox.com 1:ef70a58a6c98 681 I2C(sda,scl),
rob.meades@u-blox.com 1:ef70a58a6c98 682 _pipe(rxSize),
rob.meades@u-blox.com 1:ef70a58a6c98 683 _i2cAdr(i2cAdr)
rob.meades@u-blox.com 1:ef70a58a6c98 684 {
rob.meades@u-blox.com 1:ef70a58a6c98 685 frequency(100000);
rob.meades@u-blox.com 1:ef70a58a6c98 686 }
rob.meades@u-blox.com 1:ef70a58a6c98 687
rob.meades@u-blox.com 1:ef70a58a6c98 688 GnssI2C::~GnssI2C(void)
rob.meades@u-blox.com 1:ef70a58a6c98 689 {
rob.meades@u-blox.com 1:ef70a58a6c98 690 powerOff();
rob.meades@u-blox.com 1:ef70a58a6c98 691 }
rob.meades@u-blox.com 1:ef70a58a6c98 692
rob.meades@u-blox.com 1:ef70a58a6c98 693 bool GnssI2C::init(PinName pn)
rob.meades@u-blox.com 1:ef70a58a6c98 694 {
rob.meades@u-blox.com 1:ef70a58a6c98 695 // Power up and enable the module
rob.meades@u-blox.com 1:ef70a58a6c98 696 _powerOn();
rob.meades@u-blox.com 1:ef70a58a6c98 697
rob.meades@u-blox.com 1:ef70a58a6c98 698 if (pn != NC) {
rob.meades@u-blox.com 1:ef70a58a6c98 699 DigitalOut pin(pn, 0);
rob.meades@u-blox.com 1:ef70a58a6c98 700 ::wait_us(1);
rob.meades@u-blox.com 1:ef70a58a6c98 701 pin = 1;
rob.meades@u-blox.com 1:ef70a58a6c98 702 ::wait_ms(100);
rob.meades@u-blox.com 1:ef70a58a6c98 703 }
rob.meades@u-blox.com 1:ef70a58a6c98 704 return !I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM));
rob.meades@u-blox.com 1:ef70a58a6c98 705 }
rob.meades@u-blox.com 1:ef70a58a6c98 706
rob.meades@u-blox.com 1:ef70a58a6c98 707 int GnssI2C::getMessage(char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 708 {
rob.meades@u-blox.com 6:56eda66d585b 709 // Fill the pipe
rob.meades@u-blox.com 1:ef70a58a6c98 710 int sz = _pipe.free();
rob.meades@u-blox.com 1:ef70a58a6c98 711 if (sz)
rob.meades@u-blox.com 1:ef70a58a6c98 712 sz = _get(buf, sz);
rob.meades@u-blox.com 1:ef70a58a6c98 713 if (sz)
rob.meades@u-blox.com 1:ef70a58a6c98 714 _pipe.put(buf, sz);
rob.meades@u-blox.com 6:56eda66d585b 715 // Now parse it
rob.meades@u-blox.com 1:ef70a58a6c98 716 return _getMessage(&_pipe, buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 717 }
rob.meades@u-blox.com 1:ef70a58a6c98 718
rob.meades@u-blox.com 1:ef70a58a6c98 719 int GnssI2C::send(const char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 720 {
rob.meades@u-blox.com 1:ef70a58a6c98 721 int sent = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 722 if (len)
rob.meades@u-blox.com 1:ef70a58a6c98 723 {
rob.meades@u-blox.com 1:ef70a58a6c98 724 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
rob.meades@u-blox.com 1:ef70a58a6c98 725 sent = send(buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 726 stop();
rob.meades@u-blox.com 1:ef70a58a6c98 727 }
rob.meades@u-blox.com 1:ef70a58a6c98 728 return sent;
rob.meades@u-blox.com 1:ef70a58a6c98 729 }
rob.meades@u-blox.com 1:ef70a58a6c98 730
rob.meades@u-blox.com 1:ef70a58a6c98 731 int GnssI2C::sendNmea(const char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 732 {
rob.meades@u-blox.com 1:ef70a58a6c98 733 int sent = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 734 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
rob.meades@u-blox.com 1:ef70a58a6c98 735 sent = GnssParser::sendNmea(buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 736 stop();
rob.meades@u-blox.com 1:ef70a58a6c98 737 return sent;
rob.meades@u-blox.com 1:ef70a58a6c98 738 }
rob.meades@u-blox.com 1:ef70a58a6c98 739
rob.meades@u-blox.com 1:ef70a58a6c98 740 int GnssI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 741 {
rob.meades@u-blox.com 1:ef70a58a6c98 742 int sent = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 743 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
rob.meades@u-blox.com 1:ef70a58a6c98 744 sent = GnssParser::sendUbx(cls, id, buf, len);
rob.meades@u-blox.com 1:ef70a58a6c98 745 I2C::stop();
rob.meades@u-blox.com 1:ef70a58a6c98 746 return sent;
rob.meades@u-blox.com 1:ef70a58a6c98 747 }
rob.meades@u-blox.com 1:ef70a58a6c98 748
rob.meades@u-blox.com 1:ef70a58a6c98 749 int GnssI2C::_get(char* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 750 {
rob.meades@u-blox.com 1:ef70a58a6c98 751 int read = 0;
rob.meades@u-blox.com 1:ef70a58a6c98 752 unsigned char sz[2] = {0,0};
rob.meades@u-blox.com 1:ef70a58a6c98 753 if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) &&
rob.meades@u-blox.com 1:ef70a58a6c98 754 !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
rob.meades@u-blox.com 1:ef70a58a6c98 755 {
rob.meades@u-blox.com 1:ef70a58a6c98 756 int size = 256 * (int)sz[0] + sz[1];
rob.meades@u-blox.com 1:ef70a58a6c98 757 if (size > len)
rob.meades@u-blox.com 1:ef70a58a6c98 758 size = len;
rob.meades@u-blox.com 1:ef70a58a6c98 759 if (size > 0)
rob.meades@u-blox.com 1:ef70a58a6c98 760 {
rob.meades@u-blox.com 1:ef70a58a6c98 761 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
rob.meades@u-blox.com 1:ef70a58a6c98 762 !I2C::read(_i2cAdr,buf,size)) {
rob.meades@u-blox.com 1:ef70a58a6c98 763 read = size;
rob.meades@u-blox.com 1:ef70a58a6c98 764 }
rob.meades@u-blox.com 1:ef70a58a6c98 765 }
rob.meades@u-blox.com 1:ef70a58a6c98 766 }
rob.meades@u-blox.com 1:ef70a58a6c98 767 return read;
rob.meades@u-blox.com 1:ef70a58a6c98 768 }
rob.meades@u-blox.com 1:ef70a58a6c98 769
rob.meades@u-blox.com 1:ef70a58a6c98 770 int GnssI2C::_send(const void* buf, int len)
rob.meades@u-blox.com 1:ef70a58a6c98 771 {
rob.meades@u-blox.com 1:ef70a58a6c98 772 return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0;
rob.meades@u-blox.com 1:ef70a58a6c98 773 }
rob.meades@u-blox.com 1:ef70a58a6c98 774
rob.meades@u-blox.com 1:ef70a58a6c98 775 const char GnssI2C::REGLEN = 0xFD;
rob.meades@u-blox.com 1:ef70a58a6c98 776 const char GnssI2C::REGSTREAM = 0xFF;
rob.meades@u-blox.com 1:ef70a58a6c98 777
rob.meades@u-blox.com 1:ef70a58a6c98 778 // End Of File