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