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@1:ef70a58a6c98, 2017-04-10 (annotated)
- Committer:
- rob.meades@u-blox.com
- Date:
- Mon Apr 10 11:28:24 2017 +0100
- Revision:
- 1:ef70a58a6c98
- Child:
- 2:b10ca4aa2e5e
Add files to repo, removing temp.txt placeholder.
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 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 28 | // Create the power pins but set everything to disabled |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 29 | _gnssPower = new DigitalInOut(GNSSPWR, PIN_OUTPUT, OpenDrain, 0); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 30 | _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PushPullNoPull, 0); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 31 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 32 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 33 | GnssParser::~GnssParser(void) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 34 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 35 | // Set the power pins to lowest power state before ending |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 36 | *_gnssPower = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 37 | *_gnssEnable = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 38 | delete _gnssPower; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 39 | delete _gnssEnable; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 40 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 41 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 42 | void GnssParser::powerOff(void) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 43 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 44 | // set the GNSS into backup mode using the command RMX-LPREQ |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 45 | struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/}; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 46 | sendUbx(0x02, 0x41, &msg, sizeof(msg)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 47 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 48 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 49 | void GnssParser::_powerOn(void) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 50 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 51 | // Power up and enable the module |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 52 | *_gnssPower = 1; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 53 | wait_ms (1); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 54 | *_gnssEnable = 1; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 55 | wait_ms (1); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 56 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 57 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 58 | int GnssParser::_getMessage(Pipe<char>* pipe, char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 59 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 60 | int unkn = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 61 | int sz = pipe->size(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 62 | int fr = pipe->free(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 63 | if (len > sz) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 64 | len = sz; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 65 | while (len > 0) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 66 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 67 | // NMEA protocol |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 68 | pipe->set(unkn); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 69 | int nmea = _parseNmea(pipe,len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 70 | if ((nmea != NOT_FOUND) && (unkn > 0)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 71 | return UNKNOWN | pipe->get(buf,unkn); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 72 | if (nmea == WAIT && fr) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 73 | return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 74 | if (nmea > 0) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 75 | return NMEA | pipe->get(buf,nmea); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 76 | // UBX protocol |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 77 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 78 | pipe->set(unkn); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 79 | int ubx = _parseUbx(pipe,len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 80 | if ((ubx != NOT_FOUND) && (unkn > 0)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 81 | return UNKNOWN | pipe->get(buf,unkn); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 82 | if (ubx == WAIT && fr) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 83 | return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 84 | if (ubx > 0) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 85 | return UBX | pipe->get(buf,ubx); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 86 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 87 | // UNKNOWN |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 88 | unkn ++; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 89 | len--; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 90 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 91 | if (unkn > 0) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 92 | return UNKNOWN | pipe->get(buf,unkn); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 93 | return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 94 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 95 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 96 | int GnssParser::_parseNmea(Pipe<char>* pipe, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 97 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 98 | int o = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 99 | int c = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 100 | char ch; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 101 | if (++o > len) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 102 | if ('$' != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 103 | // this needs to be extended by crc checking |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 104 | for (;;) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 105 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 106 | if (++o > len) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 107 | ch = pipe->next(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 108 | if ('*' == ch) break; // crc delimiter |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 109 | if (!isprint(ch)) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 110 | c ^= ch; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 111 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 112 | if (++o > len) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 113 | ch = _toHex[(c >> 4) & 0xF]; // high nibble |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 114 | if (ch != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 115 | if (++o > len) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 116 | ch = _toHex[(c >> 0) & 0xF]; // low 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 | if ('\r' != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 120 | if (++o > len) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 121 | if ('\n' != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 122 | return o; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 123 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 124 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 125 | int GnssParser::_parseUbx(Pipe<char>* pipe, int l) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 126 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 127 | int o = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 128 | if (++o > l) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 129 | if ('\xB5' != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 130 | if (++o > l) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 131 | if ('\x62' != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 132 | o += 4; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 133 | if (o > l) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 134 | int i,j,ca,cb; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 135 | i = pipe->next(); ca = i; cb = ca; // cls |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 136 | i = pipe->next(); ca += i; cb += ca; // id |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 137 | i = pipe->next(); ca += i; cb += ca; // len_lsb |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 138 | j = pipe->next(); ca += j; cb += ca; // len_msb |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 139 | j = i + (j << 8); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 140 | while (j--) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 141 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 142 | if (++o > l) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 143 | i = pipe->next(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 144 | ca += i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 145 | cb += ca; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 146 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 147 | ca &= 0xFF; cb &= 0xFF; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 148 | if (++o > l) return WAIT; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 149 | if (ca != 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 (cb != pipe->next()) return NOT_FOUND; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 152 | return o; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 153 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 154 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 155 | int GnssParser::send(const char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 156 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 157 | return _send(buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 158 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 159 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 160 | int GnssParser::sendNmea(const char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 161 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 162 | char head[1] = { '$' }; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 163 | char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' }; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 164 | int i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 165 | int crc = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 166 | for (i = 0; i < len; i ++) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 167 | crc ^= *buf++; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 168 | i = _send(head, sizeof(head)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 169 | i += _send(buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 170 | tail[1] = _toHex[(crc > 4) & 0xF0]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 171 | tail[2] = _toHex[(crc > 0) & 0x0F]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 172 | i += _send(tail, sizeof(tail)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 173 | return i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 174 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 175 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 176 | int GnssParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 177 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 178 | char head[6] = { 0xB5, 0x62, cls, id, (char) len, (char) (len >> 8)}; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 179 | char crc[2]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 180 | int i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 181 | int ca = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 182 | int cb = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 183 | for (i = 2; i < 6; i ++) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 184 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 185 | ca += head[i]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 186 | cb += ca; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 187 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 188 | for (i = 0; i < len; i ++) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 189 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 190 | ca += ((char*)buf)[i]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 191 | cb += ca; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 192 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 193 | i = _send(head, sizeof(head)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 194 | i += _send(buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 195 | crc[0] = ca & 0xFF; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 196 | crc[1] = cb & 0xFF; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 197 | i += _send(crc, sizeof(crc)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 198 | return i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 199 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 200 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 201 | const char* GnssParser::findNmeaItemPos(int ix, const char* start, const char* end) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 202 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 203 | // find the start |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 204 | for (; (start < end) && (ix > 0); start ++) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 205 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 206 | if (*start == ',') |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 207 | ix --; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 208 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 209 | // found and check bounds |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 210 | if ((ix == 0) && (start < end) && |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 211 | (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n')) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 212 | return start; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 213 | else |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 214 | return NULL; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 215 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 216 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 217 | bool GnssParser::getNmeaItem(int ix, char* buf, int len, double& val) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 218 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 219 | char* end = &buf[len]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 220 | const char* pos = findNmeaItemPos(ix, buf, end); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 221 | // find the start |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 222 | if (!pos) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 223 | return false; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 224 | val = strtod(pos, &end); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 225 | // restore the last character |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 226 | return (end > pos); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 227 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 228 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 229 | bool GnssParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 230 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 231 | char* end = &buf[len]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 232 | const char* pos = findNmeaItemPos(ix, buf, end); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 233 | // find the start |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 234 | if (!pos) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 235 | return false; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 236 | val = (int)strtol(pos, &end, base); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 237 | return (end > pos); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 238 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 239 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 240 | bool GnssParser::getNmeaItem(int ix, char* buf, int len, char& val) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 241 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 242 | const char* end = &buf[len]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 243 | const char* pos = findNmeaItemPos(ix, buf, end); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 244 | // find the start |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 245 | if (!pos) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 246 | return false; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 247 | // skip leading spaces |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 248 | while ((pos < end) && isspace(*pos)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 249 | pos++; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 250 | // check bound |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 251 | if ((pos < end) && |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 252 | (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n')) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 253 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 254 | val = *pos; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 255 | return true; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 256 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 257 | return false; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 258 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 259 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 260 | bool GnssParser::getNmeaAngle(int ix, char* buf, int len, double& val) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 261 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 262 | char ch; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 263 | if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 264 | ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W'))) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 265 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 266 | val *= 0.01; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 267 | int i = (int)val; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 268 | val = (val - i) / 0.6 + i; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 269 | if (ch == 'S' || ch == 'W') |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 270 | val = -val; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 271 | return true; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 272 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 273 | return false; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 274 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 275 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 276 | 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 | 277 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 278 | // ---------------------------------------------------------------- |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 279 | // Serial Implementation |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 280 | // ---------------------------------------------------------------- |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 281 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 282 | GnssSerial::GnssSerial(PinName tx /*= GNSSTXD */, PinName rx /*= GNSSRXD */, int baudrate /*= GNSSBAUD */, |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 283 | int rxSize /*= 256 */, int txSize /*= 128 */) : |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 284 | SerialPipe(tx, rx, baudrate, rxSize, txSize) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 285 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 286 | baud(baudrate); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 287 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 288 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 289 | GnssSerial::~GnssSerial(void) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 290 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 291 | powerOff(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 292 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 293 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 294 | bool GnssSerial::init(PinName pn) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 295 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 296 | // Power up and enable the module |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 297 | _powerOn(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 298 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 299 | // send a byte to wakup the device again |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 300 | putc(0xFF); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 301 | // wait until we get some bytes |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 302 | int size = _pipeRx.size(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 303 | Timer timer; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 304 | timer.start(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 305 | while ((100 > timer.read_ms()) && (size == _pipeRx.size())) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 306 | /* nothing / just wait */; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 307 | return (size != _pipeRx.size()); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 308 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 309 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 310 | int GnssSerial::getMessage(char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 311 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 312 | return _getMessage(&_pipeRx, buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 313 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 314 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 315 | int GnssSerial::_send(const void* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 316 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 317 | return put((const char*)buf, len, true/*=blocking*/); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 318 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 319 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 320 | // ---------------------------------------------------------------- |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 321 | // I2C Implementation |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 322 | // ---------------------------------------------------------------- |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 323 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 324 | GnssI2C::GnssI2C(PinName sda /*= NC */, PinName scl /*= NC */, |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 325 | unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) : |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 326 | I2C(sda,scl), |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 327 | _pipe(rxSize), |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 328 | _i2cAdr(i2cAdr) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 329 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 330 | frequency(100000); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 331 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 332 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 333 | GnssI2C::~GnssI2C(void) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 334 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 335 | powerOff(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 336 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 337 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 338 | bool GnssI2C::init(PinName pn) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 339 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 340 | // Power up and enable the module |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 341 | _powerOn(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 342 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 343 | if (pn != NC) { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 344 | DigitalOut pin(pn, 0); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 345 | ::wait_us(1); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 346 | pin = 1; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 347 | ::wait_ms(100); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 348 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 349 | return !I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM)); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 350 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 351 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 352 | int GnssI2C::getMessage(char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 353 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 354 | // fill the pipe |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 355 | int sz = _pipe.free(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 356 | if (sz) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 357 | sz = _get(buf, sz); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 358 | if (sz) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 359 | _pipe.put(buf, sz); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 360 | // now parse it |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 361 | return _getMessage(&_pipe, buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 362 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 363 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 364 | int GnssI2C::send(const char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 365 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 366 | int sent = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 367 | if (len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 368 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 369 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 370 | sent = send(buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 371 | stop(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 372 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 373 | return sent; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 374 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 375 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 376 | int GnssI2C::sendNmea(const char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 377 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 378 | int sent = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 379 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 380 | sent = GnssParser::sendNmea(buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 381 | stop(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 382 | return sent; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 383 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 384 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 385 | int GnssI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 386 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 387 | int sent = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 388 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 389 | sent = GnssParser::sendUbx(cls, id, buf, len); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 390 | I2C::stop(); |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 391 | return sent; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 392 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 393 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 394 | int GnssI2C::_get(char* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 395 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 396 | int read = 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 397 | unsigned char sz[2] = {0,0}; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 398 | if (!I2C::write(_i2cAdr,®LEN,sizeof(REGLEN),true) && |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 399 | !I2C::read(_i2cAdr,(char*)sz,sizeof(sz))) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 400 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 401 | int size = 256 * (int)sz[0] + sz[1]; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 402 | if (size > len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 403 | size = len; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 404 | if (size > 0) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 405 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 406 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true) && |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 407 | !I2C::read(_i2cAdr,buf,size)) { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 408 | read = size; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 409 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 410 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 411 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 412 | return read; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 413 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 414 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 415 | int GnssI2C::_send(const void* buf, int len) |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 416 | { |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 417 | return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 418 | } |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 419 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 420 | const char GnssI2C::REGLEN = 0xFD; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 421 | const char GnssI2C::REGSTREAM = 0xFF; |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 422 | |
rob.meades@u-blox.com | 1:ef70a58a6c98 | 423 | // End Of File |