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