Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // NMEA parser, adapted by Michael Smith from TinyGPS v9:
18 : //
19 : // TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
20 : // Copyright (C) 2008-9 Mikal Hart
21 : // All rights reserved.
22 : //
23 : //
24 :
25 : /// @file AP_GPS_NMEA.h
26 : /// @brief NMEA protocol parser
27 : ///
28 : /// This is a lightweight NMEA parser, derived originally from the
29 : /// TinyGPS parser by Mikal Hart. It is frugal in its use of memory
30 : /// and tries to avoid unnecessary arithmetic.
31 : ///
32 : /// The parser handles GPGGA, GPRMC and GPVTG messages, and attempts to be
33 : /// robust in the face of occasional corruption in the input stream. It
34 : /// makes a basic effort to configure GPS' that are likely to be connected in
35 : /// NMEA mode (SiRF, MediaTek and ublox) to emit the correct message
36 : /// stream, but does not validate that the correct stream is being received.
37 : /// In particular, a unit emitting just GPRMC will show as having a fix
38 : /// even though no altitude data is being received.
39 : ///
40 : /// GPVTG data is parsed, but as the message may not contain the the
41 : /// qualifier field (this is common with e.g. older SiRF units) it is
42 : /// not considered a source of fix-valid information.
43 : ///
44 : #pragma once
45 :
46 : #include "AP_GPS.h"
47 : #include "GPS_Backend.h"
48 :
49 : #if AP_GPS_NMEA_ENABLED
50 : /// NMEA parser
51 : ///
52 : class AP_GPS_NMEA : public AP_GPS_Backend
53 : {
54 : friend class AP_GPS_NMEA_Test;
55 :
56 : public:
57 :
58 : using AP_GPS_Backend::AP_GPS_Backend;
59 :
60 : /// Checks the serial receive buffer for characters,
61 : /// attempts to parse NMEA data and updates internal state
62 : /// accordingly.
63 : bool read() override;
64 :
65 : static bool _detect(struct NMEA_detect_state &state, uint8_t data);
66 :
67 9 : const char *name() const override { return "NMEA"; }
68 :
69 : // driver specific health, returns true if the driver is healthy
70 : bool is_healthy(void) const override;
71 :
72 : // get lag in seconds
73 : bool get_lag(float &lag_sec) const override;
74 :
75 : #if HAL_LOGGING_ENABLED
76 : void Write_AP_Logger_Log_Startup_messages() const override;
77 : #endif
78 :
79 : private:
80 : /// Coding for the GPS sentences that the parser handles
81 : enum _sentence_types : uint16_t { //there are some more than 10 fields in some sentences , thus we have to increase these value.
82 : _GPS_SENTENCE_RMC = 32,
83 : _GPS_SENTENCE_GGA = 64,
84 : _GPS_SENTENCE_VTG = 96,
85 : _GPS_SENTENCE_HDT = 128,
86 : _GPS_SENTENCE_PHD = 138, // extension for AllyStar GPS modules
87 : _GPS_SENTENCE_THS = 160, // True heading with quality indicator, available on Trimble MB-Two
88 : _GPS_SENTENCE_KSXT = 170, // extension for Unicore, 21 fields
89 : _GPS_SENTENCE_AGRICA = 193, // extension for Unicore, 65 fields
90 : _GPS_SENTENCE_VERSIONA = 270, // extension for Unicore, version, 10 fields
91 : _GPS_SENTENCE_UNIHEADINGA = 290, // extension for Unicore, uniheadinga, 20 fields
92 : _GPS_SENTENCE_OTHER = 0
93 : };
94 :
95 : /// Update the decode state machine with a new character
96 : ///
97 : /// @param c The next character in the NMEA input stream
98 : /// @returns True if processing the character has resulted in
99 : /// an update to the GPS state
100 : ///
101 : bool _decode(char c);
102 :
103 : /// Parses the @p as a NMEA-style decimal number with
104 : /// up to 3 decimal digits.
105 : ///
106 : /// @returns The value expressed by the string in @p,
107 : /// multiplied by 100.
108 : ///
109 : static int32_t _parse_decimal_100(const char *p);
110 :
111 : /// Parses the current term as a NMEA-style degrees + minutes
112 : /// value with up to four decimal digits.
113 : ///
114 : /// This gives a theoretical resolution limit of around 1cm.
115 : ///
116 : /// @returns The value expressed by the string in _term,
117 : /// multiplied by 1e7.
118 : ///
119 : uint32_t _parse_degrees();
120 :
121 : /// Processes the current term when it has been deemed to be
122 : /// complete.
123 : ///
124 : /// Each GPS message is broken up into terms separated by commas.
125 : /// Each term is then processed by this function as it is received.
126 : ///
127 : /// @returns True if completing the term has resulted in
128 : /// an update to the GPS state.
129 : bool _term_complete();
130 :
131 : /// return true if we have a new set of NMEA messages
132 : bool _have_new_message(void);
133 :
134 : #if AP_GPS_NMEA_UNICORE_ENABLED
135 : /*
136 : parse an AGRICA field
137 : */
138 : void parse_agrica_field(uint16_t term_number, const char *term);
139 :
140 : // parse VERSIONA field
141 : void parse_versiona_field(uint16_t term_number, const char *term);
142 :
143 : #if GPS_MOVING_BASELINE
144 : // parse UNIHEADINGA field
145 : void parse_uniheadinga_field(uint16_t term_number, const char *term);
146 : #endif
147 : #endif
148 :
149 :
150 : uint8_t _parity; ///< NMEA message checksum accumulator
151 : uint32_t _crc32; ///< CRC for unicore messages
152 : bool _is_checksum_term; ///< current term is the checksum
153 : char _term[30]; ///< buffer for the current term within the current sentence
154 : uint16_t _sentence_type; ///< the sentence type currently being processed
155 : bool _is_unicore; ///< true if in a unicore '#' sentence
156 : uint16_t _term_number; ///< term index within the current sentence
157 : uint8_t _term_offset; ///< character offset with the term being received
158 : uint16_t _sentence_length;
159 : bool _sentence_done; ///< set when a sentence has been fully decoded
160 :
161 : // The result of parsing terms within a message is stored temporarily until
162 : // the message is completely processed and the checksum validated.
163 : // This avoids the need to buffer the entire message.
164 : int32_t _new_time; ///< time parsed from a term
165 : int32_t _new_date; ///< date parsed from a term
166 : int32_t _new_latitude; ///< latitude parsed from a term
167 : int32_t _new_longitude; ///< longitude parsed from a term
168 : int32_t _new_altitude; ///< altitude parsed from a term
169 : int32_t _new_speed; ///< speed parsed from a term
170 : int32_t _new_course; ///< course parsed from a term
171 : float _new_gps_yaw; ///< yaw parsed from a term
172 : uint16_t _new_hdop; ///< HDOP parsed from a term
173 : uint8_t _new_satellite_count; ///< satellite count parsed from a term
174 : uint8_t _new_quality_indicator; ///< GPS quality indicator parsed from a term
175 :
176 : uint32_t _last_RMC_ms;
177 : uint32_t _last_GGA_ms;
178 : uint32_t _last_VTG_ms;
179 : uint32_t _last_yaw_ms;
180 : uint32_t _last_vvelocity_ms;
181 : uint32_t _last_vaccuracy_ms;
182 : uint32_t _last_3D_velocity_ms;
183 : uint32_t _last_KSXT_pos_ms;
184 : uint32_t _last_AGRICA_ms;
185 : uint32_t _last_fix_ms;
186 :
187 : /// @name Init strings
188 : /// In ::init, an attempt is made to configure the GPS
189 : /// unit to send just the messages that we are interested
190 : /// in using these strings
191 : //@{
192 : static const char _SiRF_init_string[]; ///< init string for SiRF units
193 : static const char _ublox_init_string[]; ///< init string for ublox units
194 : //@}
195 :
196 : static const char _initialisation_blob[];
197 :
198 : /*
199 : the $PHD message is an extension from AllyStar that gives
200 : vertical velocity and more accuracy estimates. It is designed as
201 : a mapping from ublox UBX protocol messages to NMEA. So class 1,
202 : message 12 is a mapping to NMEA of the NAV-VELNED UBX message
203 : and contains the same fields. Class 1 message 26 is called
204 : "NAV-PVERR", but does not correspond to a UBX message
205 :
206 : example:
207 : $PHD,01,12,TIIITTITT,,245808000,0,0,0,0,0,10260304,0,0*27
208 : $PHD,01,26,TTTTTTT,,245808000,877,864,1451,11,11,17*17
209 : */
210 : struct {
211 : uint8_t msg_class;
212 : uint8_t msg_id;
213 : uint32_t itow;
214 : int32_t fields[8];
215 : } _phd;
216 :
217 : /*
218 : The KSXT message is an extension from Unicore that gives 3D velocity and yaw
219 : example: $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
220 : */
221 : struct {
222 : double fields[21];
223 : } _ksxt;
224 :
225 : #if AP_GPS_NMEA_UNICORE_ENABLED
226 : /*
227 : unicore AGRICA message parsing
228 : */
229 : struct {
230 : uint32_t start_byte;
231 : uint8_t rtk_status;
232 : uint8_t heading_status;
233 : Vector3f vel_NED;
234 : Vector3f vel_stddev;
235 : double lat, lng;
236 : float alt;
237 : uint32_t itow;
238 : float undulation;
239 : Vector3f pos_stddev;
240 : } _agrica;
241 :
242 : // unicore VERSIONA parsing
243 : struct {
244 : char type[10];
245 : char version[20];
246 : char build_date[13];
247 : } _versiona;
248 : bool _have_unicore_versiona;
249 :
250 : #if GPS_MOVING_BASELINE
251 : // unicore UNIHEADINGA parsing
252 : struct {
253 : float baseline_length;
254 : float heading;
255 : float pitch;
256 : float heading_sd;
257 : } _uniheadinga;
258 : #endif
259 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
260 : bool _expect_agrica;
261 :
262 : // last time we sent type specific config strings
263 : uint32_t last_config_ms;
264 :
265 : // send type specific config strings
266 : void send_config(void);
267 : };
268 :
269 : #if AP_GPS_NMEA_UNICORE_ENABLED && !defined(NMEA_UNICORE_SETUP)
270 : // we don't know what port the GPS may be using, so configure all 3. We need to get it sending
271 : // one message to allow the NMEA detector to run
272 : #define NMEA_UNICORE_SETUP "CONFIG COM1 230400 8 n 1\r\nCONFIG COM2 230400 8 n 1\r\nCONFIG COM3 230400 8 n 1\r\nGPGGA 0.2\r\n"
273 : #endif
274 :
275 : #endif // AP_GPS_NMEA_ENABLED
276 :
|
