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 : #pragma once
16 :
17 : #include "AP_GPS_config.h"
18 : #include "AP_GPS_FixType.h"
19 :
20 : #if AP_GPS_ENABLED
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <inttypes.h>
24 : #include <AP_Common/AP_Common.h>
25 : #include <AP_Common/Location.h>
26 : #include <AP_Param/AP_Param.h>
27 : #include "GPS_detect_state.h"
28 : #include <AP_Math/AP_Math.h>
29 : #include <AP_MSP/msp.h>
30 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
31 : #include <SITL/SIM_GPS.h>
32 : #include <GCS_MAVLink/GCS_MAVLink.h>
33 :
34 : #define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink
35 :
36 : // the number of GPS leap seconds - copied into SIM_GPS.cpp
37 : #define GPS_LEAPSECONDS_MILLIS 18000ULL
38 :
39 : #define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
40 :
41 : #if GPS_MOVING_BASELINE
42 : #include "MovingBase.h"
43 : #endif // GPS_MOVING_BASELINE
44 :
45 : class AP_GPS_Backend;
46 : class RTCM3_Parser;
47 :
48 : /// @class AP_GPS
49 : /// GPS driver main class
50 : class AP_GPS
51 : {
52 : friend class AP_GPS_Blended;
53 : friend class AP_GPS_ERB;
54 : friend class AP_GPS_GSOF;
55 : friend class AP_GPS_MAV;
56 : friend class AP_GPS_MSP;
57 : friend class AP_GPS_ExternalAHRS;
58 : friend class AP_GPS_NMEA;
59 : friend class AP_GPS_NOVA;
60 : friend class AP_GPS_PX4;
61 : friend class AP_GPS_SBF;
62 : friend class AP_GPS_SBP;
63 : friend class AP_GPS_SBP2;
64 : friend class AP_GPS_SIRF;
65 : friend class AP_GPS_UBLOX;
66 : friend class AP_GPS_Backend;
67 : friend class AP_GPS_DroneCAN;
68 : friend class AP_GPS_UBLOX_CFGv2;
69 :
70 : public:
71 : AP_GPS();
72 :
73 : /* Do not allow copies */
74 : CLASS_NO_COPY(AP_GPS);
75 :
76 478764967 : static AP_GPS *get_singleton() {
77 478764967 : return _singleton;
78 : }
79 :
80 : // allow threads to lock against GPS update
81 4323 : HAL_Semaphore &get_semaphore(void) {
82 4323 : return rsem;
83 : }
84 :
85 : // GPS driver types
86 : enum GPS_Type {
87 : GPS_TYPE_NONE = 0,
88 : GPS_TYPE_AUTO = 1,
89 : GPS_TYPE_UBLOX = 2,
90 : // GPS_TYPE_MTK = 3, // driver removed
91 : // GPS_TYPE_MTK19 = 4, // driver removed
92 : GPS_TYPE_NMEA = 5,
93 : GPS_TYPE_SIRF = 6,
94 : GPS_TYPE_HIL = 7,
95 : GPS_TYPE_SBP = 8,
96 : GPS_TYPE_UAVCAN = 9,
97 : GPS_TYPE_SBF = 10,
98 : GPS_TYPE_GSOF = 11,
99 : GPS_TYPE_ERB = 13,
100 : GPS_TYPE_MAV = 14,
101 : GPS_TYPE_NOVA = 15,
102 : GPS_TYPE_HEMI = 16, // hemisphere NMEA
103 : GPS_TYPE_UBLOX_RTK_BASE = 17,
104 : GPS_TYPE_UBLOX_RTK_ROVER = 18,
105 : GPS_TYPE_MSP = 19,
106 : GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
107 : GPS_TYPE_EXTERNAL_AHRS = 21,
108 : GPS_TYPE_UAVCAN_RTK_BASE = 22,
109 : GPS_TYPE_UAVCAN_RTK_ROVER = 23,
110 : GPS_TYPE_UNICORE_NMEA = 24,
111 : GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
112 : GPS_TYPE_SBF_DUAL_ANTENNA = 26,
113 : #if AP_SIM_GPS_ENABLED
114 : GPS_TYPE_SITL = 100,
115 : #endif
116 : };
117 :
118 : // convenience methods for working out what general type an instance is:
119 : bool is_rtk_base(uint8_t instance) const;
120 : bool is_rtk_rover(uint8_t instance) const;
121 :
122 : // return the instance whose moving baseline rover solution provides the
123 : // yaw reported for this instance by gps_yaw_deg
124 : uint8_t yaw_source_instance(uint8_t instance) const;
125 :
126 : // params for an instance:
127 : class Params {
128 : public:
129 : // Constructor
130 : Params(void);
131 :
132 : AP_Enum<GPS_Type> type;
133 : AP_Int16 gnss_mode;
134 : AP_Int16 rate_ms; // this parameter should always be accessed using get_rate_ms()
135 : AP_Vector3f antenna_offset;
136 : AP_Int16 delay_ms;
137 : AP_Int8 com_port;
138 : #if HAL_ENABLE_DRONECAN_DRIVERS
139 : AP_Int32 node_id;
140 : AP_Int32 override_node_id;
141 : #endif
142 : #if GPS_MOVING_BASELINE
143 : MovingBase mb_params;
144 : #endif // GPS_MOVING_BASELINE
145 :
146 : static const struct AP_Param::GroupInfo var_info[];
147 : };
148 :
149 : // Aliases for AP_GPS_FixType values, for backwards compatibility with scripting
150 : static constexpr uint8_t NO_GPS = (uint8_t)AP_GPS_FixType::NO_GPS;
151 : static constexpr uint8_t NO_FIX = (uint8_t)AP_GPS_FixType::NONE;
152 : static constexpr uint8_t GPS_OK_FIX_2D = (uint8_t)AP_GPS_FixType::FIX_2D;
153 : static constexpr uint8_t GPS_OK_FIX_3D = (uint8_t)AP_GPS_FixType::FIX_3D;
154 : static constexpr uint8_t GPS_OK_FIX_3D_DGPS = (uint8_t)AP_GPS_FixType::DGPS;
155 : static constexpr uint8_t GPS_OK_FIX_3D_RTK_FLOAT = (uint8_t)AP_GPS_FixType::RTK_FLOAT;
156 : static constexpr uint8_t GPS_OK_FIX_3D_RTK_FIXED = (uint8_t)AP_GPS_FixType::RTK_FIXED;
157 : static constexpr uint8_t GPS_OK_FIX_TYPE_STATIC = (uint8_t)AP_GPS_FixType::STATIC;
158 : static constexpr uint8_t GPS_OK_FIX_TYPE_PPP = (uint8_t)AP_GPS_FixType::PPP;
159 :
160 : // GPS navigation engine settings. Not all GPS receivers support
161 : // this
162 : enum GPS_Engine_Setting {
163 : GPS_ENGINE_NONE = -1,
164 : GPS_ENGINE_PORTABLE = 0,
165 : GPS_ENGINE_STATIONARY = 2,
166 : GPS_ENGINE_PEDESTRIAN = 3,
167 : GPS_ENGINE_AUTOMOTIVE = 4,
168 : GPS_ENGINE_SEA = 5,
169 : GPS_ENGINE_AIRBORNE_1G = 6,
170 : GPS_ENGINE_AIRBORNE_2G = 7,
171 : GPS_ENGINE_AIRBORNE_4G = 8
172 : };
173 :
174 : // role for auto-config
175 : enum GPS_Role {
176 : GPS_ROLE_NORMAL,
177 : GPS_ROLE_MB_BASE,
178 : GPS_ROLE_MB_ROVER,
179 : };
180 :
181 : // GPS Covariance Types matching ROS2 sensor_msgs/msg/NavSatFix
182 : enum class CovarianceType : uint8_t {
183 : UNKNOWN = 0, ///< The GPS does not support any accuracy metrics
184 : APPROXIMATED = 1, ///< The accuracy is approximated through metrics such as HDOP/VDOP
185 : DIAGONAL_KNOWN = 2, ///< The diagonal (east, north, up) components of covariance are reported by the GPS
186 : KNOWN = 3, ///< The full covariance array is reported by the GPS
187 : };
188 :
189 : /*
190 : The GPS_State structure is filled in by the backend driver as it
191 : parses each message from the GPS.
192 : */
193 : struct GPS_State {
194 : uint8_t instance; // the instance number of this GPS
195 :
196 : // all the following fields must all be filled by the backend driver
197 : AP_GPS_FixType status; ///< driver fix status
198 : uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
199 : uint16_t time_week; ///< GPS week number
200 : Location location; ///< last fix location
201 : float ground_speed; ///< ground speed in m/s
202 : float ground_course; ///< ground course in degrees, wrapped 0-360
203 : float gps_yaw; ///< GPS derived yaw information, if available (degrees)
204 : uint32_t gps_yaw_time_ms; ///< timestamp of last GPS yaw reading
205 : bool gps_yaw_configured; ///< GPS is configured to provide yaw
206 : uint16_t hdop; ///< horizontal dilution of precision, scaled by a factor of 100 (155 means the HDOP value is 1.55)
207 : uint16_t vdop; ///< vertical dilution of precision, scaled by a factor of 100 (155 means the VDOP value is 1.55)
208 : uint8_t num_sats; ///< Number of visible satellites
209 : Vector3f velocity; ///< 3D velocity in m/s, in NED format
210 : float speed_accuracy; ///< 3D velocity RMS accuracy estimate in m/s
211 : float horizontal_accuracy; ///< horizontal RMS accuracy estimate in m
212 : float vertical_accuracy; ///< vertical RMS accuracy estimate in m
213 : float gps_yaw_accuracy; ///< heading accuracy of the GPS in degrees
214 : bool have_vertical_velocity; ///< does GPS give vertical velocity? Set to true only once available.
215 : bool have_speed_accuracy; ///< does GPS give speed accuracy? Set to true only once available.
216 : bool have_horizontal_accuracy; ///< does GPS give horizontal position accuracy? Set to true only once available.
217 : bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available.
218 : bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
219 : bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available
220 : float undulation; //<height that WGS84 is above AMSL at the current location
221 : bool have_undulation; ///<do we have a value for the undulation
222 : uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
223 : bool announced_detection; ///< true once we have announced GPS has been seen to the user
224 : uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
225 : bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated
226 : uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected
227 :
228 : // all the following fields must only all be filled by RTK capable backend drivers
229 : uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds
230 : uint16_t rtk_week_number; ///< GPS Week Number of last baseline
231 : uint32_t rtk_age_ms; ///< GPS age of last baseline correction in milliseconds (0 when no corrections, 0xFFFFFFFF indicates overflow)
232 : uint8_t rtk_num_sats; ///< Current number of satellites used for RTK calculation
233 : uint8_t rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED
234 : int32_t rtk_baseline_x_mm; ///< Current baseline in ECEF x or NED north component in mm
235 : int32_t rtk_baseline_y_mm; ///< Current baseline in ECEF y or NED east component in mm
236 : int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm
237 : uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
238 : int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses
239 :
240 : // UBX Relative Position and Heading message information
241 : float relPosHeading; ///< Reported Heading in degrees
242 : float relPosLength; ///< Reported Position horizontal distance in meters
243 : float relPosD; ///< Reported Vertical distance in meters
244 : float accHeading; ///< Reported Heading Accuracy in degrees
245 : uint32_t relposheading_ts; ///< True if new data has been received since last time it was false
246 :
247 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
248 : // body-frame antenna offset used to calculate gps_yaw from a moving
249 : // baseline, zero when gps_yaw is not derived from a moving baseline.
250 : // gps_yaw is calculated assuming this offset is horizontal, so
251 : // consumers with an attitude estimate can use this offset to correct
252 : // gps_yaw for vehicle roll and pitch
253 : Vector3f mb_yaw_offset;
254 : #endif
255 : };
256 :
257 : /// Startup initialisation.
258 : void init();
259 :
260 : // ethod for APPPeriph to set the default type for the first GPS instance:
261 : void set_default_type_for_gps1(uint8_t default_type) {
262 : params[0].type.set_default(default_type);
263 : }
264 :
265 : /// Update GPS state based on possible bytes received from the module.
266 : /// This routine must be called periodically (typically at 10Hz or
267 : /// more) to process incoming data.
268 : void update(void);
269 :
270 : // Pass mavlink data to message handlers (for MAV type)
271 : void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
272 : #if HAL_MSP_GPS_ENABLED
273 : void handle_msp(const MSP::msp_gps_data_message_t &pkt);
274 : #endif
275 : #if AP_EXTERNAL_AHRS_ENABLED
276 : // Retrieve the first instance ID that is configured as type GPS_TYPE_EXTERNAL_AHRS.
277 : // Can be used by external AHRS systems that only report one GPS to get the instance ID.
278 : // Returns true if an instance was found, false otherwise.
279 : bool get_first_external_instance(uint8_t& instance) const WARN_IF_UNUSED;
280 : void handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt, const uint8_t instance);
281 : #endif
282 :
283 : // Accessor functions
284 :
285 : // return number of active GPS sensors. Note that if the first GPS
286 : // is not present but the 2nd is then we return 2. Note that a blended
287 : // GPS solution is treated as an additional sensor.
288 : uint8_t num_sensors(void) const;
289 :
290 : // Return the index of the primary sensor which is the index of the sensor contributing to
291 : // the output. A blended solution is available as an additional instance
292 41845713 : uint8_t primary_sensor(void) const {
293 41845713 : return primary_instance;
294 : }
295 :
296 : /// Query GPS status
297 391130159 : AP_GPS_FixType status(uint8_t instance) const {
298 391130159 : if (_force_disable_gps && state[instance].status > AP_GPS_FixType::NONE) {
299 12461 : return AP_GPS_FixType::NONE;
300 : }
301 391117698 : return state[instance].status;
302 : }
303 265817971 : AP_GPS_FixType status(void) const {
304 265817971 : return status(primary_instance);
305 : }
306 :
307 : // return a single human-presentable character representing the
308 : // fix type. For space-constrained human-readable displays
309 0 : char status_onechar(void) const {
310 0 : switch (status()) {
311 0 : case AP_GPS_FixType::NO_GPS:
312 0 : return ' ';
313 0 : case AP_GPS_FixType::NONE:
314 0 : return '-';
315 0 : case AP_GPS_FixType::FIX_2D:
316 0 : return '2';
317 0 : case AP_GPS_FixType::FIX_3D:
318 0 : return '3';
319 0 : case AP_GPS_FixType::DGPS:
320 0 : return '4';
321 0 : case AP_GPS_FixType::RTK_FLOAT:
322 0 : return '5';
323 0 : case AP_GPS_FixType::RTK_FIXED:
324 0 : return '6';
325 0 : case AP_GPS_FixType::STATIC:
326 0 : return '7';
327 0 : case AP_GPS_FixType::PPP:
328 0 : return '8';
329 : }
330 : // should never reach here; compiler flags guarantees this.
331 0 : return '?';
332 : }
333 :
334 : // Query the highest status this GPS supports (always reports FIX_3D for the blended GPS)
335 : AP_GPS_FixType highest_supported_status(uint8_t instance) const WARN_IF_UNUSED;
336 :
337 : // location of last fix
338 126812793 : const Location &location(uint8_t instance) const {
339 126812793 : return state[instance].location;
340 : }
341 1532558 : const Location &location() const {
342 1532558 : return location(primary_instance);
343 : }
344 :
345 : // get the difference between WGS84 and AMSL. A positive value means
346 : // the AMSL height is higher than WGS84 ellipsoid height
347 : bool get_undulation(uint8_t instance, float &undulation) const;
348 :
349 : // get the difference between WGS84 and AMSL. A positive value means
350 : // the AMSL height is higher than WGS84 ellipsoid height
351 13 : bool get_undulation(float &undulation) const {
352 13 : return get_undulation(primary_instance, undulation);
353 : }
354 :
355 : // report speed accuracy
356 : bool speed_accuracy(uint8_t instance, float &sacc) const;
357 1306596 : bool speed_accuracy(float &sacc) const {
358 1306596 : return speed_accuracy(primary_instance, sacc);
359 : }
360 :
361 : bool horizontal_accuracy(uint8_t instance, float &hacc) const;
362 1306596 : bool horizontal_accuracy(float &hacc) const {
363 1306596 : return horizontal_accuracy(primary_instance, hacc);
364 : }
365 :
366 : bool vertical_accuracy(uint8_t instance, float &vacc) const;
367 1306596 : bool vertical_accuracy(float &vacc) const {
368 1306596 : return vertical_accuracy(primary_instance, vacc);
369 : }
370 :
371 : CovarianceType position_covariance(const uint8_t instance, Matrix3f& cov) const WARN_IF_UNUSED;
372 :
373 : // 3D velocity in NED format
374 202618594 : const Vector3f &velocity(uint8_t instance) const {
375 202618594 : return state[instance].velocity;
376 : }
377 78460363 : const Vector3f &velocity() const {
378 78460363 : return velocity(primary_instance);
379 : }
380 :
381 : // ground speed in m/s
382 46000803 : float ground_speed(uint8_t instance) const {
383 46000803 : return state[instance].ground_speed;
384 : }
385 44189463 : float ground_speed() const {
386 44189463 : return ground_speed(primary_instance);
387 : }
388 :
389 : // ground course in degrees
390 43676418 : float ground_course(uint8_t instance) const {
391 43676418 : return state[instance].ground_course;
392 : }
393 41853492 : float ground_course() const {
394 41853492 : return ground_course(primary_instance);
395 : }
396 : // ground course in centi-degrees
397 11591 : int32_t ground_course_cd(uint8_t instance) const {
398 11591 : return ground_course(instance) * 100;
399 : }
400 11591 : int32_t ground_course_cd() const {
401 11591 : return ground_course_cd(primary_instance);
402 : }
403 :
404 : // yaw in degrees if available
405 : bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const;
406 0 : bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
407 0 : return gps_yaw_deg(primary_instance, yaw_deg, accuracy_deg, time_ms);
408 : }
409 :
410 : // number of locked satellites
411 164717320 : uint8_t num_sats(uint8_t instance) const {
412 164717320 : return state[instance].num_sats;
413 : }
414 39437131 : uint8_t num_sats() const {
415 39437131 : return num_sats(primary_instance);
416 : }
417 :
418 : // GPS time of week in milliseconds
419 689442 : uint32_t time_week_ms(uint8_t instance) const {
420 689442 : return state[instance].time_week_ms;
421 : }
422 66 : uint32_t time_week_ms() const {
423 66 : return time_week_ms(primary_instance);
424 : }
425 :
426 : // GPS week
427 689442 : uint16_t time_week(uint8_t instance) const {
428 689442 : return state[instance].time_week;
429 : }
430 66 : uint16_t time_week() const {
431 66 : return time_week(primary_instance);
432 : }
433 :
434 : // horizontal dilution of precision
435 125283045 : uint16_t get_hdop(uint8_t instance) const {
436 125283045 : return state[instance].hdop;
437 : }
438 2856 : uint16_t get_hdop() const {
439 2856 : return get_hdop(primary_instance);
440 : }
441 :
442 : // vertical dilution of precision
443 1813314 : uint16_t get_vdop(uint8_t instance) const {
444 1813314 : return state[instance].vdop;
445 : }
446 1980 : uint16_t get_vdop() const {
447 1980 : return get_vdop(primary_instance);
448 : }
449 :
450 : // the time we got our last fix in system milliseconds. This is
451 : // used when calculating how far we might have moved since that fix
452 41597038 : uint32_t last_fix_time_ms(uint8_t instance) const {
453 41597038 : return timing[instance].last_fix_time_ms;
454 : }
455 40475080 : uint32_t last_fix_time_ms(void) const {
456 40475080 : return last_fix_time_ms(primary_instance);
457 : }
458 :
459 : // the time we last processed a message in milliseconds. This is
460 : // used to indicate that we have new GPS data to process
461 163129162 : uint32_t last_message_time_ms(uint8_t instance) const {
462 163129162 : return timing[instance].last_message_time_ms;
463 : }
464 38970931 : uint32_t last_message_time_ms(void) const {
465 38970931 : return last_message_time_ms(primary_instance);
466 : }
467 :
468 : // system time delta between the last two reported positions
469 689376 : uint16_t last_message_delta_time_ms(uint8_t instance) const {
470 689376 : return timing[instance].delta_time_ms;
471 : }
472 : uint16_t last_message_delta_time_ms(void) const {
473 : return last_message_delta_time_ms(primary_instance);
474 : }
475 :
476 : // return true if the GPS supports vertical velocity values
477 124158233 : bool have_vertical_velocity(uint8_t instance) const {
478 124158233 : return state[instance].have_vertical_velocity;
479 : }
480 2 : bool have_vertical_velocity(void) const {
481 2 : return have_vertical_velocity(primary_instance);
482 : }
483 :
484 : // return true if the GPS currently has yaw available
485 124160971 : bool have_gps_yaw(uint8_t instance) const {
486 124160971 : return !_force_disable_gps_yaw && state[instance].have_gps_yaw;
487 : }
488 0 : bool have_gps_yaw(void) const {
489 0 : return have_gps_yaw(primary_instance);
490 : }
491 :
492 : // return true if the GPS is configured to provide yaw. This will
493 : // be true if we expect the GPS to provide yaw, even if it
494 : // currently is not able to provide yaw
495 1121958 : bool have_gps_yaw_configured(uint8_t instance) const {
496 1121958 : return state[instance].gps_yaw_configured;
497 : }
498 :
499 : // the expected lag (in seconds) in the position and velocity readings from the gps
500 : // return true if the GPS hardware configuration is known or the lag parameter has been set manually
501 : bool get_lag(uint8_t instance, float &lag_sec) const;
502 38475091 : bool get_lag(float &lag_sec) const {
503 38475091 : return get_lag(primary_instance, lag_sec);
504 : }
505 :
506 : // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
507 : const Vector3f &get_antenna_offset(uint8_t instance) const;
508 :
509 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
510 : // return the body-frame moving baseline antenna offset used to calculate
511 : // the yaw returned by gps_yaw_deg for this instance, zero when that yaw
512 : // is not derived from a moving baseline
513 : const Vector3f &get_mb_yaw_offset(uint8_t instance) const;
514 : #endif
515 :
516 : // lock out a GPS port, allowing another application to use the port
517 : void lock_port(uint8_t instance, bool locked);
518 :
519 : //MAVLink Status Sending
520 : void send_mavlink_gps_raw(mavlink_channel_t chan);
521 : void send_mavlink_gps2_raw(mavlink_channel_t chan);
522 :
523 : void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst);
524 :
525 : // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS
526 : bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED;
527 : void broadcast_first_configuration_failure_reason(void) const;
528 :
529 : // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
530 : bool all_consistent(float &distance) const;
531 :
532 : // handle sending of initialisation strings to the GPS - only used by backends
533 : void send_blob_start(uint8_t instance);
534 : void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
535 : void send_blob_update(uint8_t instance);
536 :
537 : // return last fix time since the 1/1/1970 in microseconds
538 : uint64_t time_epoch_usec(uint8_t instance) const;
539 1306662 : uint64_t time_epoch_usec(void) const {
540 1306662 : return time_epoch_usec(primary_instance);
541 : }
542 :
543 : uint64_t last_message_epoch_usec(uint8_t instance) const;
544 : uint64_t last_message_epoch_usec() const {
545 : return last_message_epoch_usec(primary_instance);
546 : }
547 :
548 : // convert GPS week and millis to unix epoch in ms
549 : static uint64_t istate_time_to_epoch_ms(uint16_t gps_week, uint32_t gps_ms);
550 :
551 : static const struct AP_Param::GroupInfo var_info[];
552 :
553 : #if HAL_LOGGING_ENABLED
554 : void Write_AP_Logger_Log_Startup_messages();
555 : #endif
556 :
557 : // indicate which bit in LOG_BITMASK indicates gps logging enabled
558 2092 : void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; }
559 :
560 : // report if the gps is healthy (this is defined as existing, an update at a rate greater than 4Hz,
561 : // as well as any driver specific behaviour)
562 : bool is_healthy(uint8_t instance) const;
563 1121120 : bool is_healthy(void) const { return is_healthy(primary_instance); }
564 :
565 : // returns true if all GPS instances have passed all final arming checks/state changes
566 : bool prepare_for_arming(void);
567 :
568 : // returns true if all GPS backend drivers are OK with the concept
569 : // of the vehicle arming. this is for backends to be able to
570 : // spout pre arm error messages
571 : bool pre_arm_checks(char failure_msg[], uint16_t failure_msg_len);
572 :
573 : // returns false if any GPS drivers are not performing their logging appropriately
574 : bool logging_failed(void) const;
575 :
576 1121120 : bool logging_present(void) const { return _raw_data != 0; }
577 2242240 : bool logging_enabled(void) const { return _raw_data != 0; }
578 :
579 : // used to disable GPS for GPS failure testing in flight
580 6 : void force_disable(bool disable) {
581 6 : _force_disable_gps = disable;
582 6 : }
583 :
584 : // used to disable GPS yaw for GPS failure testing in flight
585 0 : void set_force_disable_yaw(bool disable) {
586 0 : _force_disable_gps_yaw = disable;
587 0 : }
588 :
589 : // handle possibly fragmented RTCM injection data
590 : void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
591 :
592 : // get configured type by instance
593 288853634 : GPS_Type get_type(uint8_t instance) const {
594 288853634 : return instance>=ARRAY_SIZE(params) ? GPS_Type::GPS_TYPE_NONE : params[instance].type;
595 : }
596 :
597 : // get iTOW, if supported, zero otherwie
598 : uint32_t get_itow(uint8_t instance) const;
599 :
600 : bool get_error_codes(uint8_t instance, uint32_t &error_codes) const;
601 0 : bool get_error_codes(uint32_t &error_codes) const { return get_error_codes(primary_instance, error_codes); }
602 :
603 : enum class SBAS_Mode : int8_t {
604 : Disabled = 0,
605 : Enabled = 1,
606 : DoNotChange = 2,
607 : };
608 :
609 : #if GPS_MOVING_BASELINE
610 : // methods used by UAVCAN GPS driver and AP_Periph for moving baseline
611 : void inject_MBL_data(uint8_t* data, uint16_t length);
612 : bool get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading) WARN_IF_UNUSED;
613 : bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len);
614 : void clear_RTCMV3();
615 : #endif // GPS_MOVING_BASELINE
616 :
617 : #if !AP_GPS_BLENDED_ENABLED
618 : uint8_t get_auto_switch_type() const { return _auto_switch; }
619 : #endif
620 :
621 : // Inject a packet of raw binary to a GPS
622 : void inject_data(const uint8_t *data, uint16_t len);
623 :
624 : protected:
625 :
626 : // configuration parameters
627 : Params params[GPS_MAX_INSTANCES];
628 : AP_Int8 _navfilter;
629 : AP_Int8 _auto_switch;
630 : AP_Int16 _sbp_logmask;
631 : AP_Int8 _inject_to;
632 : uint32_t _last_instance_swap_ms;
633 : AP_Enum<SBAS_Mode> _sbas_mode;
634 : AP_Int8 _min_elevation;
635 : AP_Int8 _raw_data;
636 : AP_Int8 _save_config;
637 : AP_Int8 _auto_config;
638 : AP_Int8 _blend_mask;
639 : AP_Int16 _driver_options;
640 : AP_Int8 _primary;
641 :
642 : uint32_t _log_gps_bit = -1;
643 :
644 : enum DriverOptions : int16_t {
645 : UBX_MBUseUart2 = (1U << 0U),
646 : SBF_UseBaseForYaw = (1U << 1U),
647 : UBX_Use115200 = (1U << 2U),
648 : UAVCAN_MBUseDedicatedBus = (1 << 3U),
649 : HeightEllipsoid = (1U << 4),
650 : GPSL5HealthOverride = (1U << 5),
651 : AlwaysRTCMDecode = (1U << 6),
652 : DisableRTCMDecode = (1U << 7),
653 : ForceUBXConfigV2 = (1U << 8U)
654 : };
655 :
656 : // check if an option is set
657 50952083 : bool option_set(const DriverOptions option) const {
658 50952083 : return (uint16_t(_driver_options.get()) & uint16_t(option)) != 0;
659 : }
660 :
661 : private:
662 : static AP_GPS *_singleton;
663 : HAL_Semaphore rsem;
664 :
665 : // returns the desired gps update rate in milliseconds
666 : // this does not provide any guarantee that the GPS is updating at the requested
667 : // rate it is simply a helper for use in the backends for determining what rate
668 : // they should be configuring the GPS to run at
669 : uint16_t get_rate_ms(uint8_t instance) const;
670 :
671 : struct GPS_timing {
672 : // the time we got our last fix in system milliseconds
673 : uint32_t last_fix_time_ms;
674 :
675 : // the time we got our last message in system milliseconds
676 : uint32_t last_message_time_ms;
677 :
678 : // delta time between the last pair of GPS updates in system milliseconds
679 : uint16_t delta_time_ms;
680 :
681 : // count of delayed frames
682 : uint8_t delayed_count;
683 :
684 : // the average time delta
685 : float average_delta_ms;
686 : };
687 : // Note allowance for an additional instance to contain blended data
688 : GPS_timing timing[GPS_MAX_INSTANCES];
689 : GPS_State state[GPS_MAX_INSTANCES];
690 : AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
691 : AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
692 :
693 : /// primary GPS instance
694 : uint8_t primary_instance;
695 :
696 : /// number of GPS instances present
697 : uint8_t num_instances;
698 :
699 : // which ports are locked
700 : uint8_t locked_ports;
701 :
702 : // state of auto-detection process, per instance
703 : struct detect_state {
704 : uint32_t last_baud_change_ms;
705 : uint8_t current_baud;
706 : uint32_t probe_baud;
707 : bool auto_detected_baud;
708 : #if AP_GPS_UBLOX_ENABLED
709 : struct UBLOX_detect_state ublox_detect_state;
710 : #endif
711 : #if AP_GPS_SIRF_ENABLED
712 : struct SIRF_detect_state sirf_detect_state;
713 : #endif
714 : #if AP_GPS_NMEA_ENABLED
715 : struct NMEA_detect_state nmea_detect_state;
716 : #endif
717 : #if AP_GPS_SBP_ENABLED
718 : struct SBP_detect_state sbp_detect_state;
719 : #endif
720 : #if AP_GPS_SBP2_ENABLED
721 : struct SBP2_detect_state sbp2_detect_state;
722 : #endif
723 : #if AP_GPS_ERB_ENABLED
724 : struct ERB_detect_state erb_detect_state;
725 : #endif
726 : } detect_state[GPS_MAX_RECEIVERS];
727 :
728 : struct {
729 : const char *blob;
730 : uint16_t remaining;
731 : } initblob_state[GPS_MAX_RECEIVERS];
732 :
733 : static const uint32_t _baudrates[];
734 : static const char _initialisation_blob[];
735 : static const char _initialisation_raw_blob[];
736 :
737 : void detect_instance(uint8_t instance);
738 : // run detection step for one GPS instance. If this finds a GPS then it
739 : // will return it - otherwise nullptr
740 : AP_GPS_Backend *_detect_instance(uint8_t instance);
741 :
742 : void update_instance(uint8_t instance);
743 :
744 : // fold timing[instance].delta_time_ms into the delayed-frame counters
745 : // used by is_healthy(). Only called when that delta is a real
746 : // measurement of the interval between two messages
747 : void update_frame_timing_health(uint8_t instance);
748 :
749 : /*
750 : buffer for re-assembling RTCM data for GPS injection.
751 : The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
752 : 1 bit for "is fragmented"
753 : 2 bits for fragment number
754 : 5 bits for sequence number
755 :
756 : The rtcm_buffer is allocated on first use. Once a block of data
757 : is successfully reassembled it is injected into all active GPS
758 : backends. This assumes we don't want more than 4*180=720 bytes
759 : in a RTCM data block
760 : */
761 : struct rtcm_buffer {
762 : uint8_t fragments_received;
763 : uint8_t sequence;
764 : uint8_t fragment_count;
765 : uint16_t total_length;
766 : uint8_t buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4];
767 : } *rtcm_buffer;
768 :
769 : struct {
770 : uint16_t fragments_used;
771 : uint16_t fragments_discarded;
772 : } rtcm_stats;
773 :
774 : // re-assemble GPS_RTCM_DATA message
775 : void handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg);
776 : void handle_gps_inject(const mavlink_message_t &msg);
777 :
778 : //Inject a packet of raw binary to a GPS
779 : void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
780 :
781 : #if AP_GPS_BLENDED_ENABLED
782 : bool _output_is_blended; // true when a blended GPS solution being output
783 : #endif
784 :
785 : bool should_log() const;
786 :
787 : bool needs_uart(GPS_Type type) const;
788 :
789 : #if GPS_MAX_RECEIVERS > 1
790 : /// Update primary instance
791 : void update_primary(void);
792 : #endif
793 :
794 : // helper function for mavlink gps yaw
795 : uint16_t gps_yaw_cdeg(uint8_t instance) const;
796 :
797 : // Auto configure types
798 : enum GPS_AUTO_CONFIG {
799 : GPS_AUTO_CONFIG_DISABLE = 0,
800 : GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY = 1,
801 : GPS_AUTO_CONFIG_ENABLE_ALL = 2,
802 : GPS_AUTO_CLEAR_CONFIG_UBLOX_SERIAL_ONLY = 3, // then acts like GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY
803 : };
804 :
805 : enum class GPSAutoSwitch {
806 : NONE = 0,
807 : USE_BEST = 1,
808 : BLEND = 2,
809 : //USE_SECOND = 3, deprecated for new primary param
810 : USE_PRIMARY_IF_3D_FIX = 4,
811 : };
812 :
813 : // used for flight testing with GPS loss
814 : bool _force_disable_gps;
815 :
816 : // used for flight testing with GPS yaw loss
817 : bool _force_disable_gps_yaw;
818 :
819 : // logging support
820 : void Write_GPS(uint8_t instance);
821 :
822 : #if AP_GPS_RTCM_DECODE_ENABLED
823 : /*
824 : per mavlink channel RTCM decoder, enabled with RTCM decode
825 : option in GPS_DRV_OPTIONS
826 : */
827 : struct {
828 : RTCM3_Parser *parsers[MAVLINK_COMM_NUM_BUFFERS];
829 : uint32_t sent_crc[32];
830 : uint8_t sent_idx;
831 : uint16_t seen_mav_channels;
832 : } rtcm;
833 : bool parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt);
834 : #endif
835 :
836 : void convert_parameters();
837 : };
838 :
839 : namespace AP {
840 : AP_GPS &gps();
841 : };
842 :
843 : #endif // AP_GPS_ENABLED
|
