Wajahat Abbas / gnss

gnss

gnss.cpp@34:d6a65d4c902a, 2019-07-29 (annotated)

Committer:
wajahat.abbas@u-blox.com
Date:
Mon Jul 29 14:24:22 2019 +0500
Revision:
34:d6a65d4c902a
Parent:
33:75163fa7e453 
Fixed build error on generic applications

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