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