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