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 : #include "AP_GPS_config.h"
16 :
17 : #if AP_GPS_ENABLED
18 :
19 : #include "AP_GPS.h"
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_Math/AP_Math.h>
24 : #include <AP_Notify/AP_Notify.h>
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_BoardConfig/AP_BoardConfig.h>
27 : #include <AP_RTC/AP_RTC.h>
28 : #include <climits>
29 : #include <AP_SerialManager/AP_SerialManager.h>
30 :
31 : #include "AP_GPS_NOVA.h"
32 : #include "AP_GPS_Blended.h"
33 : #include "AP_GPS_ERB.h"
34 : #include "AP_GPS_GSOF.h"
35 : #include "AP_GPS_NMEA.h"
36 : #include "AP_GPS_SBF.h"
37 : #include "AP_GPS_SBP.h"
38 : #include "AP_GPS_SBP2.h"
39 : #include "AP_GPS_SIRF.h"
40 : #include "AP_GPS_UBLOX.h"
41 : #include "AP_GPS_MAV.h"
42 : #include "AP_GPS_MSP.h"
43 : #include "AP_GPS_ExternalAHRS.h"
44 : #include "GPS_Backend.h"
45 : #if AP_SIM_GPS_ENABLED
46 : #include "AP_GPS_SITL.h"
47 : #endif
48 :
49 : #if HAL_ENABLE_DRONECAN_DRIVERS
50 : #include <AP_CANManager/AP_CANManager.h>
51 : #include <AP_DroneCAN/AP_DroneCAN.h>
52 : #include "AP_GPS_DroneCAN.h"
53 : #endif
54 :
55 : #include <AP_AHRS/AP_AHRS.h>
56 : #include <AP_Logger/AP_Logger.h>
57 : #include "AP_GPS_FixType.h"
58 :
59 : #if AP_GPS_RTCM_DECODE_ENABLED
60 : #include "RTCM3_Parser.h"
61 : #endif
62 :
63 : #if !AP_GPS_BLENDED_ENABLED
64 : #if defined(GPS_BLENDED_INSTANCE)
65 : #error GPS_BLENDED_INSTANCE should not be defined when AP_GPS_BLENDED_ENABLED is false
66 : #endif
67 : #endif
68 :
69 : #define GPS_RTK_INJECT_TO_ALL 127
70 : #ifndef GPS_MAX_RATE_MS
71 : #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
72 : #endif
73 : #define GPS_BAUD_TIME_MS 1200
74 : #define GPS_TIMEOUT_MS 4000u
75 :
76 : extern const AP_HAL::HAL &hal;
77 :
78 : // baudrates to try to detect GPSes with
79 : const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U, 460800U};
80 :
81 : // initialisation blobs to send to the GPS to try to get it into the
82 : // right mode.
83 : const char AP_GPS::_initialisation_blob[] =
84 : #if AP_GPS_UBLOX_ENABLED
85 : UBLOX_SET_BINARY_230400
86 : #endif
87 : #if AP_GPS_SIRF_ENABLED
88 : SIRF_SET_BINARY
89 : #endif
90 : #if AP_GPS_NMEA_UNICORE_ENABLED
91 : NMEA_UNICORE_SETUP
92 : #endif
93 : "" // to compile we need *some_initialiser if all backends compiled out
94 : ;
95 :
96 : #if HAL_GCS_ENABLED
97 : // ensure that AP_GPS_FixType is 1:1 with the mavlink
98 : // numbers of the same fix type. This allows us to do a simple cast
99 : // from one to the other when sending GPS mavlink messages, rather
100 : // than having some sort of mapping function from our internal
101 : // enumeration into the mavlink enumeration. Doing things this way
102 : // has two advantages - in the future we could add that mapping
103 : // function and change our enumeration, and the other is that it
104 : // allows us to build the GPS library without having the mavlink
105 : // headers built (for example, in AP_Periph we shouldn't need mavlink
106 : // headers).
107 : static_assert((uint32_t)AP_GPS_FixType::NO_GPS == (uint32_t)GPS_FIX_TYPE_NO_GPS, "NO_GPS incorrect");
108 : static_assert((uint32_t)AP_GPS_FixType::NONE == (uint32_t)GPS_FIX_TYPE_NO_FIX, "NO_FIX incorrect");
109 : static_assert((uint32_t)AP_GPS_FixType::FIX_2D == (uint32_t)GPS_FIX_TYPE_2D_FIX, "FIX_2D incorrect");
110 : static_assert((uint32_t)AP_GPS_FixType::FIX_3D == (uint32_t)GPS_FIX_TYPE_3D_FIX, "FIX_3D incorrect");
111 : static_assert((uint32_t)AP_GPS_FixType::DGPS == (uint32_t)GPS_FIX_TYPE_DGPS, "FIX_DGPS incorrect");
112 : static_assert((uint32_t)AP_GPS_FixType::RTK_FLOAT == (uint32_t)GPS_FIX_TYPE_RTK_FLOAT, "FIX_RTK_FLOAT incorrect");
113 : static_assert((uint32_t)AP_GPS_FixType::RTK_FIXED == (uint32_t)GPS_FIX_TYPE_RTK_FIXED, "FIX_RTK_FIXED incorrect");
114 : static_assert((uint32_t)AP_GPS_FixType::STATIC == (uint32_t)GPS_FIX_TYPE_STATIC, "FIX_STATIC incorrect");
115 : static_assert((uint32_t)AP_GPS_FixType::PPP == (uint32_t)GPS_FIX_TYPE_PPP, "FIX_PPP incorrect");
116 : #endif
117 :
118 : AP_GPS *AP_GPS::_singleton;
119 :
120 : // table of user settable parameters
121 : const AP_Param::GroupInfo AP_GPS::var_info[] = {
122 :
123 : // 0 was GPS_TYPE
124 :
125 : // 1 was GPS_TYPE2
126 :
127 : // @Param: _NAVFILTER
128 : // @DisplayName: Navigation filter setting
129 : // @Description: Navigation filter engine setting
130 : // @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G
131 : // @User: Advanced
132 : AP_GROUPINFO("_NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
133 :
134 : #if GPS_MAX_RECEIVERS > 1
135 : // @Param: _AUTO_SWITCH
136 : // @DisplayName: Automatic Switchover Setting
137 : // @Description: Automatic switchover to GPS reporting best lock, 1:UseBest selects the GPS with highest status, if both are equal the GPS with highest satellite count is used 4:Use primary if 3D fix or better, will revert to 'UseBest' behaviour if 3D fix is lost on primary
138 : // @Values: 0:Use primary, 1:UseBest, 2:Blend, 4:Use primary if 3D fix or better
139 : // @User: Advanced
140 : AP_GROUPINFO("_AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST),
141 : #endif
142 :
143 : // 4 was _MIN_GPS, removed Feb 2024
144 :
145 : // @Param: _SBAS_MODE
146 : // @DisplayName: SBAS Mode
147 : // @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful.
148 : // @Values: 0:Disabled,1:Enabled,2:NoChange
149 : // @User: Advanced
150 : AP_GROUPINFO("_SBAS_MODE", 5, AP_GPS, _sbas_mode, 2),
151 :
152 : // @Param: _MIN_ELEV
153 : // @DisplayName: Minimum elevation
154 : // @Description: This sets the minimum elevation of satellites above the horizon for them to be used for navigation. Setting this to -100 leaves the minimum elevation set to the GPS modules default.
155 : // @Range: -100 90
156 : // @Units: deg
157 : // @User: Advanced
158 : AP_GROUPINFO("_MIN_ELEV", 6, AP_GPS, _min_elevation, -100),
159 :
160 : // @Param: _INJECT_TO
161 : // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets
162 : // @Description: The GGS can send raw serial packets to inject data to multiple GPSes.
163 : // @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all
164 : // @User: Advanced
165 : AP_GROUPINFO("_INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL),
166 :
167 : #if AP_GPS_SBP2_ENABLED || AP_GPS_SBP_ENABLED
168 : // @Param: _SBP_LOGMASK
169 : // @DisplayName: Swift Binary Protocol Logging Mask
170 : // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged
171 : // @Values: 0:None (0x0000),-1:All (0xFFFF),-256:External only (0xFF00)
172 : // @User: Advanced
173 : AP_GROUPINFO("_SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, -256),
174 : #endif //AP_GPS_SBP2_ENABLED || AP_GPS_SBP_ENABLED
175 :
176 : // @Param: _RAW_DATA
177 : // @DisplayName: Raw data logging
178 : // @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into logger; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming
179 : // @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only)
180 : // @RebootRequired: True
181 : // @User: Advanced
182 : AP_GROUPINFO("_RAW_DATA", 9, AP_GPS, _raw_data, 0),
183 :
184 : // 10 was GPS_GNSS_MODE
185 :
186 : // @Param: _SAVE_CFG
187 : // @DisplayName: Save GPS configuration
188 : // @Description: Determines whether the configuration for this GPS should be written to non-volatile memory on the GPS. Currently working for UBlox 6 series and above.
189 : // @Values: 0:Do not save config,1:Save config,2:Save only when needed
190 : // @User: Advanced
191 : AP_GROUPINFO("_SAVE_CFG", 11, AP_GPS, _save_config, 2),
192 :
193 : // 12 was GPS_GNSS_MODE2
194 :
195 : // @Param: _AUTO_CONFIG
196 : // @DisplayName: Automatic GPS configuration
197 : // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings
198 : // @Values: 0:Disables automatic configuration,1:Enable automatic configuration for Serial GPSes only,2:Enable automatic configuration for DroneCAN as well,3:Clear all configurations not set by ardupilot (UBlox only)
199 : // @User: Advanced
200 : AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
201 :
202 : // 14 was GPS_RATE_MS
203 :
204 : // 15 was GPS_RATE_MS2
205 :
206 : // 16 was GPS_POS1
207 :
208 : // 17 was GPS_POS2
209 :
210 : // 18 was GPS_DELAY_MS
211 :
212 : // 19 was GPS_DELAY_MS2
213 :
214 : #if AP_GPS_BLENDED_ENABLED
215 : // @Param: _BLEND_MASK
216 : // @DisplayName: Multi GPS Blending Mask
217 : // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2(Blend)
218 : // @Bitmask: 0:Horiz Pos,1:Vert Pos,2:Speed
219 : // @User: Advanced
220 : AP_GROUPINFO("_BLEND_MASK", 20, AP_GPS, _blend_mask, 5),
221 :
222 : // @Param: _BLEND_TC
223 : // @DisplayName: Blending time constant
224 : // @Description: Controls the slowest time constant applied to the calculation of GPS position and height offsets used to adjust different GPS receivers for steady state position differences.
225 : // @Units: s
226 : // @Range: 5.0 30.0
227 : // @User: Advanced
228 : // Had key 21, no longer used
229 : #endif
230 :
231 : // @Param: _DRV_OPTIONS
232 : // @DisplayName: driver options
233 : // @Description: Additional backend specific options
234 : // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200 on ublox,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel, 8:Force UBlox Config Get/Set for configuration then automatic configuration for Serial GPSes only
235 : // @User: Advanced
236 : AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
237 :
238 : // 23 was GPS_COM_PORT
239 :
240 : // 24 was GPS_COM_PORT2
241 :
242 : // 25 was GPS_MB1_*
243 :
244 : // 26 was GPS_MB2_*
245 :
246 : #if GPS_MAX_RECEIVERS > 1
247 : // @Param: _PRIMARY
248 : // @DisplayName: Primary GPS
249 : // @Description: This GPS will be used when GPS_AUTO_SWITCH is 0 and used preferentially with GPS_AUTO_SWITCH = 4.
250 : // @Increment: 1
251 : // @Values: 0:FirstGPS, 1:SecondGPS
252 : // @User: Advanced
253 : AP_GROUPINFO("_PRIMARY", 27, AP_GPS, _primary, 0),
254 : #endif
255 :
256 : // 28 was GPS_CAN_NODEID1
257 :
258 : // 29 was GPS_CAN_NODEID2
259 :
260 : // 30 was GPS1_CAN_OVRIDE
261 :
262 : // 31 was GPS2_CAN_OVRIDE
263 :
264 : // @Group: 1_
265 : // @Path: AP_GPS_Params.cpp
266 : AP_SUBGROUPINFO(params[0], "1_", 32, AP_GPS, AP_GPS::Params),
267 :
268 : #if GPS_MAX_RECEIVERS > 1
269 : // @Group: 2_
270 : // @Path: AP_GPS_Params.cpp
271 : AP_SUBGROUPINFO(params[1], "2_", 33, AP_GPS, AP_GPS::Params),
272 : #endif
273 :
274 : AP_GROUPEND
275 : };
276 :
277 : // constructor
278 14686 : AP_GPS::AP_GPS()
279 : {
280 : static_assert((sizeof(_initialisation_blob) * (CHAR_BIT + 2)) < (4800 * GPS_BAUD_TIME_MS * 1e-3),
281 : "GPS initilisation blob is too large to be completely sent before the baud rate changes");
282 :
283 2098 : AP_Param::setup_object_defaults(this, var_info);
284 :
285 2098 : if (_singleton != nullptr) {
286 0 : AP_HAL::panic("AP_GPS must be singleton");
287 : }
288 2098 : _singleton = this;
289 2098 : }
290 :
291 : // return true if a specific type of GPS uses a UART
292 4184 : bool AP_GPS::needs_uart(GPS_Type type) const
293 : {
294 4184 : switch (type) {
295 2145 : case GPS_TYPE_NONE:
296 : case GPS_TYPE_HIL:
297 : case GPS_TYPE_UAVCAN:
298 : case GPS_TYPE_UAVCAN_RTK_BASE:
299 : case GPS_TYPE_UAVCAN_RTK_ROVER:
300 : case GPS_TYPE_MAV:
301 : case GPS_TYPE_MSP:
302 : case GPS_TYPE_EXTERNAL_AHRS:
303 2145 : return false;
304 2039 : default:
305 2039 : break;
306 : }
307 2039 : return true;
308 : }
309 :
310 : /// Startup initialisation.
311 2092 : void AP_GPS::init()
312 : {
313 : // set the default for the first GPS according to define:
314 2092 : params[0].type.set_default(HAL_GPS1_TYPE_DEFAULT);
315 :
316 : // PARAMETER_CONVERSION - Added: Jan-2026
317 6276 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
318 4184 : params[i].gnss_mode.convert_parameter_width(AP_PARAM_INT8);
319 : }
320 :
321 2092 : convert_parameters();
322 :
323 : // search for serial ports with gps protocol
324 2092 : const auto &serial_manager = AP::serialmanager();
325 2092 : uint8_t uart_idx = 0;
326 6276 : for (uint8_t i=0; i<ARRAY_SIZE(params) && i<ARRAY_SIZE(_port); i++) {
327 4184 : if (needs_uart(params[i].type)) {
328 2039 : _port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, uart_idx);
329 2039 : uart_idx++;
330 : }
331 : }
332 2092 : _last_instance_swap_ms = 0;
333 :
334 : // prep the state instance fields
335 8368 : for (uint8_t i = 0; i < GPS_MAX_INSTANCES; i++) {
336 6276 : state[i].instance = i;
337 : }
338 :
339 : // sanity check update rate
340 8368 : for (uint8_t i=0; i<ARRAY_SIZE(params); i++) {
341 6276 : auto &rate_ms = params[i].rate_ms;
342 6276 : if (rate_ms <= 0 || rate_ms > GPS_MAX_RATE_MS) {
343 0 : rate_ms.set(GPS_MAX_RATE_MS);
344 : }
345 : }
346 :
347 : // create the blended instance if appropriate:
348 : #if AP_GPS_BLENDED_ENABLED
349 2092 : drivers[GPS_BLENDED_INSTANCE] = NEW_NOTHROW AP_GPS_Blended(*this, params[GPS_BLENDED_INSTANCE], state[GPS_BLENDED_INSTANCE], timing[GPS_BLENDED_INSTANCE]);
350 : #endif
351 2092 : }
352 :
353 2092 : void AP_GPS::convert_parameters()
354 : {
355 : // find GPS's top level key
356 : uint16_t k_param_gps_key;
357 2092 : if (!AP_Param::find_top_level_key_by_pointer(this, k_param_gps_key)) {
358 0 : return;
359 : }
360 :
361 : // table parameters to convert without scaling
362 : static const AP_Param::ConversionInfo conversion_info[] {
363 : // PARAMETER_CONVERSION - Added: Mar-2024 for ArduPilot-4.6
364 : { k_param_gps_key, 0, AP_PARAM_INT8, "GPS1_TYPE" },
365 : { k_param_gps_key, 1, AP_PARAM_INT8, "GPS2_TYPE" },
366 : { k_param_gps_key, 10, AP_PARAM_INT8, "GPS1_GNSS_MODE" },
367 : { k_param_gps_key, 12, AP_PARAM_INT8, "GPS2_GNSS_MODE" },
368 : { k_param_gps_key, 14, AP_PARAM_INT16, "GPS1_RATE_MS" },
369 : { k_param_gps_key, 15, AP_PARAM_INT16, "GPS2_RATE_MS" },
370 : { k_param_gps_key, 16, AP_PARAM_VECTOR3F, "GPS1_POS" },
371 : { k_param_gps_key, 17, AP_PARAM_VECTOR3F, "GPS2_POS" },
372 : { k_param_gps_key, 18, AP_PARAM_INT16, "GPS1_DELAY_MS" },
373 : { k_param_gps_key, 19, AP_PARAM_INT16, "GPS2_DELAY_MS" },
374 : #if AP_GPS_SBF_ENABLED
375 : { k_param_gps_key, 23, AP_PARAM_INT8, "GPS1_COM_PORT" },
376 : { k_param_gps_key, 24, AP_PARAM_INT8, "GPS2_COM_PORT" },
377 : #endif
378 :
379 : #if HAL_ENABLE_DRONECAN_DRIVERS
380 : { k_param_gps_key, 28, AP_PARAM_INT32, "GPS1_CAN_NODEID" },
381 : { k_param_gps_key, 29, AP_PARAM_INT32, "GPS2_CAN_NODEID" },
382 : { k_param_gps_key, 30, AP_PARAM_INT32, "GPS1_CAN_OVRIDE" },
383 : { k_param_gps_key, 31, AP_PARAM_INT32, "GPS2_CAN_OVRIDE" },
384 : #endif
385 2092 : };
386 2092 : AP_Param::convert_old_parameters(conversion_info, ARRAY_SIZE(conversion_info));
387 :
388 : #if GPS_MOVING_BASELINE
389 : // convert old MovingBaseline parameters
390 : // PARAMETER_CONVERSION - Added: Mar-2024 for ArduPilot-4.6
391 6276 : for (uint8_t i=0; i<MIN(2, GPS_MAX_RECEIVERS); i++) {
392 : // the old _MB parameters were 25 and 26:
393 4184 : const uint8_t old_index = 25 + i;
394 4184 : AP_Param::convert_class(k_param_gps_key, ¶ms[i].mb_params, params[i].mb_params.var_info, old_index, false);
395 : }
396 : #endif // GPS_MOVING_BASELINE
397 : }
398 :
399 : // return number of active GPS sensors. Note that if the first GPS
400 : // is not present but the 2nd is then we return 2. Note that a blended
401 : // GPS solution is treated as an additional sensor.
402 41257693 : uint8_t AP_GPS::num_sensors(void) const
403 : {
404 : #if AP_GPS_BLENDED_ENABLED
405 41257693 : if (_output_is_blended) {
406 128860 : return num_instances+1;
407 : }
408 : #endif
409 41128833 : return num_instances;
410 : }
411 :
412 126586824 : bool AP_GPS::speed_accuracy(uint8_t instance, float &sacc) const
413 : {
414 126586824 : if (state[instance].have_speed_accuracy) {
415 42011682 : sacc = state[instance].speed_accuracy;
416 42011682 : return true;
417 : }
418 84575142 : return false;
419 : }
420 :
421 126586785 : bool AP_GPS::horizontal_accuracy(uint8_t instance, float &hacc) const
422 : {
423 126586785 : if (state[instance].have_horizontal_accuracy) {
424 42123261 : hacc = state[instance].horizontal_accuracy;
425 42123261 : return true;
426 : }
427 84463524 : return false;
428 : }
429 :
430 126586785 : bool AP_GPS::vertical_accuracy(uint8_t instance, float &vacc) const
431 : {
432 126586785 : if (state[instance].have_vertical_accuracy) {
433 42123261 : vacc = state[instance].vertical_accuracy;
434 42123261 : return true;
435 : }
436 84463524 : return false;
437 : }
438 :
439 0 : AP_GPS::CovarianceType AP_GPS::position_covariance(const uint8_t instance, Matrix3f& cov) const
440 : {
441 0 : AP_GPS::CovarianceType cov_type = AP_GPS::CovarianceType::UNKNOWN;
442 0 : cov.zero();
443 : float hacc, vacc;
444 0 : if (horizontal_accuracy(instance, hacc) && vertical_accuracy(instance, vacc))
445 : {
446 0 : cov_type = AP_GPS::CovarianceType::DIAGONAL_KNOWN;
447 0 : const auto hacc_variance = hacc * hacc;
448 0 : cov[0][0] = hacc_variance;
449 0 : cov[1][1] = hacc_variance;
450 0 : cov[2][2] = vacc * vacc;
451 : }
452 : // There may be a receiver that implements hdop+vdop but not accuracy
453 : // If so, there could be a condition here to attempt to calculate it
454 :
455 0 : return cov_type;
456 : }
457 :
458 :
459 : /**
460 : convert GPS week and milliseconds to unix epoch in milliseconds
461 : */
462 8059364 : uint64_t AP_GPS::istate_time_to_epoch_ms(uint16_t gps_week, uint32_t gps_ms)
463 : {
464 8059364 : uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms;
465 8059364 : return fix_time_ms;
466 : }
467 :
468 : /**
469 : calculate current time since the unix epoch in microseconds
470 : */
471 8186665 : uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const
472 : {
473 8186665 : const GPS_State &istate = state[instance];
474 8186665 : if ((istate.last_gps_time_ms == 0 && istate.last_corrected_gps_time_us == 0) || istate.time_week == 0) {
475 127301 : return 0;
476 : }
477 : uint64_t fix_time_ms;
478 : // add in the time since the last fix message
479 8059364 : if (istate.last_corrected_gps_time_us != 0) {
480 8053612 : fix_time_ms = istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms());
481 8053612 : return (fix_time_ms*1000ULL) + (AP_HAL::micros64() - istate.last_corrected_gps_time_us);
482 : } else {
483 5752 : fix_time_ms = istate_time_to_epoch_ms(istate.time_week, istate.time_week_ms);
484 5752 : return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL;
485 : }
486 : }
487 :
488 : /**
489 : calculate last message time since the unix epoch in microseconds
490 : */
491 0 : uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const
492 : {
493 0 : const GPS_State &istate = state[instance];
494 0 : if (istate.time_week == 0) {
495 0 : return 0;
496 : }
497 0 : return istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms()) * 1000ULL;
498 : }
499 :
500 : /*
501 : send some more initialisation string bytes if there is room in the
502 : UART transmit buffer
503 : */
504 11389 : void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size)
505 : {
506 11389 : initblob_state[instance].blob = _blob;
507 11389 : initblob_state[instance].remaining = size;
508 11389 : }
509 :
510 : /*
511 : initialise state for sending binary blob initialisation strings. We
512 : may choose different blobs not just based on type but also based on
513 : parameters.
514 : */
515 10366 : void AP_GPS::send_blob_start(uint8_t instance)
516 : {
517 10366 : const auto type = params[instance].type;
518 :
519 : #if AP_GPS_UBLOX_ENABLED
520 10366 : if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) {
521 : static const char blob[] = UBLOX_SET_BINARY_115200;
522 0 : send_blob_start(instance, blob, sizeof(blob));
523 0 : return;
524 : }
525 : #endif // AP_GPS_UBLOX_ENABLED
526 :
527 : #if GPS_MOVING_BASELINE && AP_GPS_UBLOX_ENABLED
528 20732 : if ((type == GPS_TYPE_UBLOX_RTK_BASE ||
529 20732 : type == GPS_TYPE_UBLOX_RTK_ROVER) &&
530 0 : !option_set(DriverOptions::UBX_MBUseUart2)) {
531 : // we use 460800 when doing moving baseline as we need
532 : // more bandwidth. We don't do this if using UART2, as
533 : // in that case the RTCMv3 data doesn't go over the
534 : // link to the flight controller
535 : static const char blob[] = UBLOX_SET_BINARY_460800;
536 0 : send_blob_start(instance, blob, sizeof(blob));
537 0 : return;
538 : }
539 : #endif
540 :
541 : // the following devices don't have init blobs:
542 10366 : const char *blob = nullptr;
543 10366 : uint32_t blob_size = 0;
544 10366 : switch (GPS_Type(type)) {
545 : #if AP_GPS_SBF_ENABLED
546 6 : case GPS_TYPE_SBF:
547 : case GPS_TYPE_SBF_DUAL_ANTENNA:
548 : #endif //AP_GPS_SBF_ENABLED
549 : #if AP_GPS_GSOF_ENABLED
550 : case GPS_TYPE_GSOF:
551 : #endif //AP_GPS_GSOF_ENABLED
552 : #if AP_GPS_NOVA_ENABLED
553 : case GPS_TYPE_NOVA:
554 : #endif //AP_GPS_NOVA_ENABLED
555 : #if AP_SIM_GPS_ENABLED
556 : case GPS_TYPE_SITL:
557 : #endif // AP_SIM_GPS_ENABLED
558 : // none of these GPSs have initialisation blobs
559 6 : break;
560 10360 : default:
561 : // send combined initialisation blob, on the assumption that the
562 : // GPS units will parse what they need and ignore the data they
563 : // don't understand:
564 10360 : blob = _initialisation_blob;
565 10360 : blob_size = sizeof(_initialisation_blob);
566 10360 : break;
567 : }
568 :
569 10366 : send_blob_start(instance, blob, blob_size);
570 : }
571 :
572 : /*
573 : send some more initialisation string bytes if there is room in the
574 : UART transmit buffer
575 : */
576 7439295 : void AP_GPS::send_blob_update(uint8_t instance)
577 : {
578 : // exit immediately if no uart for this instance
579 7439295 : if (instance >= ARRAY_SIZE(_port) ||
580 7439295 : _port[instance] == nullptr) {
581 57383 : return;
582 : }
583 :
584 7381912 : if (initblob_state[instance].remaining == 0) {
585 7371552 : return;
586 : }
587 :
588 : // see if we can write some more of the initialisation blob
589 10360 : const uint16_t n = MIN(_port[instance]->txspace(),
590 10360 : initblob_state[instance].remaining);
591 10360 : const size_t written = _port[instance]->write((const uint8_t*)initblob_state[instance].blob, n);
592 10360 : initblob_state[instance].blob += written;
593 10360 : initblob_state[instance].remaining -= written;
594 : }
595 :
596 : /*
597 : run detection step for one GPS instance. If this finds a GPS then it
598 : will fill in drivers[instance] and change state[instance].status
599 : from NO_GPS to NO_FIX.
600 : */
601 541513 : void AP_GPS::detect_instance(uint8_t instance)
602 : {
603 541513 : const uint32_t now = AP_HAL::millis();
604 :
605 541513 : state[instance].status = AP_GPS_FixType::NO_GPS;
606 541513 : state[instance].hdop = GPS_UNKNOWN_DOP;
607 541513 : state[instance].vdop = GPS_UNKNOWN_DOP;
608 :
609 541513 : AP_GPS_Backend *new_gps = _detect_instance(instance);
610 541513 : if (new_gps == nullptr) {
611 540415 : return;
612 : }
613 :
614 1098 : state[instance].status = AP_GPS_FixType::NONE;
615 1098 : drivers[instance] = new_gps;
616 1098 : timing[instance].last_message_time_ms = now;
617 1098 : timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
618 :
619 1098 : new_gps->broadcast_gps_type();
620 : }
621 :
622 : /*
623 : run detection step for one GPS instance. If this finds a GPS then it
624 : will return it - otherwise nullptr
625 : */
626 541513 : AP_GPS_Backend *AP_GPS::_detect_instance(const uint8_t instance)
627 : {
628 541513 : struct detect_state *dstate = &detect_state[instance];
629 :
630 541513 : const auto type = params[instance].type;
631 541513 : auto *port = (instance < ARRAY_SIZE(_port)) ? _port[instance] : nullptr;
632 :
633 541513 : switch (GPS_Type(type)) {
634 : // user has to explicitly set the MAV type, do not use AUTO
635 : // do not try to detect the MAV type, assume it's there
636 2 : case GPS_TYPE_MAV:
637 : #if AP_GPS_MAV_ENABLED
638 2 : dstate->auto_detected_baud = false; // specified, not detected
639 2 : return NEW_NOTHROW AP_GPS_MAV(*this, params[instance], state[instance], nullptr);
640 : #endif //AP_GPS_MAV_ENABLED
641 :
642 : // user has to explicitly set the UAVCAN type, do not use AUTO
643 75 : case GPS_TYPE_UAVCAN:
644 : case GPS_TYPE_UAVCAN_RTK_BASE:
645 : case GPS_TYPE_UAVCAN_RTK_ROVER:
646 : #if AP_GPS_DRONECAN_ENABLED
647 75 : dstate->auto_detected_baud = false; // specified, not detected
648 75 : return AP_GPS_DroneCAN::probe(*this, state[instance]);
649 : #endif
650 : return nullptr; // We don't do anything here if UAVCAN is not supported
651 : #if HAL_MSP_GPS_ENABLED
652 1 : case GPS_TYPE_MSP:
653 1 : dstate->auto_detected_baud = false; // specified, not detected
654 1 : return NEW_NOTHROW AP_GPS_MSP(*this, params[instance], state[instance], nullptr);
655 : #endif
656 : #if AP_EXTERNAL_AHRS_ENABLED
657 1015 : case GPS_TYPE_EXTERNAL_AHRS:
658 1015 : if (AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::GPS) >= 0) {
659 59 : dstate->auto_detected_baud = false; // specified, not detected
660 59 : return NEW_NOTHROW AP_GPS_ExternalAHRS(*this, params[instance], state[instance], nullptr);
661 : }
662 956 : break;
663 : #endif
664 : #if AP_GPS_GSOF_ENABLED
665 1 : case GPS_TYPE_GSOF:
666 1 : dstate->auto_detected_baud = false; // specified, not detected
667 1 : return NEW_NOTHROW AP_GPS_GSOF(*this, params[instance], state[instance], port);
668 : #endif //AP_GPS_GSOF_ENABLED
669 540419 : default:
670 540419 : break;
671 : }
672 :
673 541375 : if (port == nullptr) {
674 : // UART not available
675 3119 : return nullptr;
676 : }
677 :
678 : // all remaining drivers automatically cycle through baud rates to detect
679 : // the correct baud rate, and should have the selected baud broadcast
680 538256 : dstate->auto_detected_baud = true;
681 538256 : const uint32_t now = AP_HAL::millis();
682 :
683 538256 : if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
684 : // try the next baud rate
685 : // incrementing like this will skip the first element in array of bauds
686 : // this is okay, and relied upon
687 10505 : if (dstate->probe_baud == 0) {
688 2031 : dstate->probe_baud = port->get_baud_rate();
689 : } else {
690 8474 : dstate->current_baud++;
691 8474 : if (dstate->current_baud == ARRAY_SIZE(_baudrates)) {
692 3 : dstate->current_baud = 0;
693 : }
694 8474 : dstate->probe_baud = _baudrates[dstate->current_baud];
695 : }
696 10505 : uint16_t rx_size=0, tx_size=0;
697 10505 : if (type == GPS_TYPE_UBLOX_RTK_ROVER) {
698 64 : tx_size = 2048;
699 : }
700 10505 : if (type == GPS_TYPE_UBLOX_RTK_BASE) {
701 73 : rx_size = 2048;
702 : }
703 10505 : port->begin(dstate->probe_baud, rx_size, tx_size);
704 10505 : port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
705 10505 : dstate->last_baud_change_ms = now;
706 :
707 10505 : if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
708 10366 : send_blob_start(instance);
709 : }
710 : }
711 :
712 538256 : if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
713 531540 : send_blob_update(instance);
714 : }
715 :
716 538256 : switch (GPS_Type(type)) {
717 : #if AP_GPS_SBF_ENABLED
718 : // by default the sbf/trimble gps outputs no data on its port, until configured.
719 1 : case GPS_TYPE_SBF:
720 : case GPS_TYPE_SBF_DUAL_ANTENNA:
721 1 : return NEW_NOTHROW AP_GPS_SBF(*this, params[instance], state[instance], port);
722 : #endif //AP_GPS_SBF_ENABLED
723 : #if AP_GPS_NOVA_ENABLED
724 4 : case GPS_TYPE_NOVA:
725 4 : return NEW_NOTHROW AP_GPS_NOVA(*this, params[instance], state[instance], port);
726 : #endif //AP_GPS_NOVA_ENABLED
727 :
728 : #if AP_SIM_GPS_ENABLED
729 1 : case GPS_TYPE_SITL:
730 1 : return NEW_NOTHROW AP_GPS_SITL(*this, params[instance], state[instance], port);
731 : #endif // AP_SIM_GPS_ENABLED
732 :
733 538250 : default:
734 538250 : break;
735 : }
736 :
737 538250 : if (initblob_state[instance].remaining != 0) {
738 : // don't run detection engines if we haven't sent out the initblobs
739 0 : return nullptr;
740 : }
741 :
742 538250 : uint16_t bytecount = MIN(8192U, port->available());
743 :
744 23990816 : while (bytecount-- > 0) {
745 23453595 : const uint8_t data = port->read();
746 : (void)data; // if all backends are compiled out then "data" is unused
747 :
748 : #if AP_GPS_UBLOX_ENABLED
749 23867666 : if ((type == GPS_TYPE_AUTO ||
750 414071 : type == GPS_TYPE_UBLOX) &&
751 23054896 : ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
752 23054896 : (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) ||
753 46943442 : _baudrates[dstate->current_baud] == 230400) &&
754 36252 : AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
755 1007 : return NEW_NOTHROW AP_GPS_UBLOX(*this, params[instance], state[instance], port, GPS_ROLE_NORMAL);
756 : }
757 :
758 23452588 : const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800;
759 46712024 : if ((type == GPS_TYPE_UBLOX_RTK_BASE ||
760 23259436 : type == GPS_TYPE_UBLOX_RTK_ROVER) &&
761 46712600 : _baudrates[dstate->current_baud] == ublox_mb_required_baud &&
762 576 : AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
763 : GPS_Role role;
764 16 : if (type == GPS_TYPE_UBLOX_RTK_BASE) {
765 8 : role = GPS_ROLE_MB_BASE;
766 : } else {
767 8 : role = GPS_ROLE_MB_ROVER;
768 : }
769 16 : return NEW_NOTHROW AP_GPS_UBLOX(*this, params[instance], state[instance], port, role);
770 : }
771 : #endif // AP_GPS_UBLOX_ENABLED
772 : #if AP_GPS_SBP2_ENABLED
773 46491090 : if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
774 23038518 : AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
775 1 : return NEW_NOTHROW AP_GPS_SBP2(*this, params[instance], state[instance], port);
776 : }
777 : #endif //AP_GPS_SBP2_ENABLED
778 : #if AP_GPS_SBP_ENABLED
779 46491088 : if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
780 23038517 : AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
781 1 : return NEW_NOTHROW AP_GPS_SBP(*this, params[instance], state[instance], port);
782 : }
783 : #endif //AP_GPS_SBP_ENABLED
784 : #if AP_GPS_SIRF_ENABLED
785 46491086 : if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SIRF) &&
786 23038516 : AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
787 0 : return NEW_NOTHROW AP_GPS_SIRF(*this, params[instance], state[instance], port);
788 : }
789 : #endif
790 : #if AP_GPS_ERB_ENABLED
791 46491086 : if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_ERB) &&
792 23038516 : AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
793 0 : return NEW_NOTHROW AP_GPS_ERB(*this, params[instance], state[instance], port);
794 : }
795 : #endif // AP_GPS_ERB_ENABLED
796 : #if AP_GPS_NMEA_ENABLED
797 46904841 : if ((type == GPS_TYPE_NMEA ||
798 46904542 : type == GPS_TYPE_HEMI ||
799 : #if AP_GPS_NMEA_UNICORE_ENABLED
800 46904542 : type == GPS_TYPE_UNICORE_NMEA ||
801 46904542 : type == GPS_TYPE_UNICORE_MOVINGBASE_NMEA ||
802 : #endif
803 70357411 : type == GPS_TYPE_ALLYSTAR) &&
804 299 : AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
805 4 : return NEW_NOTHROW AP_GPS_NMEA(*this, params[instance], state[instance], port);
806 : }
807 : #endif //AP_GPS_NMEA_ENABLED
808 : }
809 :
810 537221 : return nullptr;
811 : }
812 :
813 0 : AP_GPS_FixType AP_GPS::highest_supported_status(uint8_t instance) const
814 : {
815 0 : if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
816 0 : return drivers[instance]->highest_supported_status();
817 : }
818 0 : return AP_GPS_FixType::FIX_3D;
819 : }
820 :
821 : #if HAL_LOGGING_ENABLED
822 712873 : bool AP_GPS::should_log() const
823 : {
824 712873 : AP_Logger *logger = AP_Logger::get_singleton();
825 712873 : if (logger == nullptr) {
826 0 : return false;
827 : }
828 712873 : if (_log_gps_bit == (uint32_t)-1) {
829 0 : return false;
830 : }
831 712873 : if (!logger->should_log(_log_gps_bit)) {
832 23444 : return false;
833 : }
834 689429 : return true;
835 : }
836 : #endif
837 :
838 :
839 : /*
840 : keep count of delayed frames and average frame delay for health
841 : reporting. Must only be called when timing[instance].delta_time_ms is a
842 : real measurement of the gap between two messages
843 : */
844 710185 : void AP_GPS::update_frame_timing_health(uint8_t instance)
845 : {
846 710185 : const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
847 710185 : GPS_timing &t = timing[instance];
848 :
849 710185 : if (t.delta_time_ms > gps_max_delta_ms) {
850 163 : t.delayed_count++;
851 : } else {
852 710022 : t.delayed_count = 0;
853 : }
854 710185 : if (t.delta_time_ms < 2000) {
855 710141 : if (t.average_delta_ms <= 0) {
856 1086 : t.average_delta_ms = t.delta_time_ms;
857 : } else {
858 709055 : t.average_delta_ms = 0.98f * t.average_delta_ms + 0.02f * t.delta_time_ms;
859 : }
860 : }
861 710185 : }
862 :
863 : /*
864 : update one GPS instance. This should be called at 10Hz or greater
865 : */
866 14913256 : void AP_GPS::update_instance(uint8_t instance)
867 : {
868 14913256 : const auto type = params[instance].type;
869 14913256 : if (type == GPS_TYPE_HIL) {
870 : // in HIL, leave info alone
871 7958598 : return;
872 : }
873 14913256 : if (type == GPS_TYPE_NONE) {
874 : // not enabled
875 7417085 : state[instance].status = AP_GPS_FixType::NO_GPS;
876 7417085 : state[instance].hdop = GPS_UNKNOWN_DOP;
877 7417085 : state[instance].vdop = GPS_UNKNOWN_DOP;
878 7417085 : return;
879 : }
880 7496171 : if (locked_ports & (1U<<instance)) {
881 : // the port is locked by another driver
882 0 : return;
883 : }
884 :
885 7496171 : if (drivers[instance] == nullptr) {
886 : // we don't yet know the GPS type of this one, or it has timed
887 : // out and needs to be re-initialised
888 541513 : detect_instance(instance);
889 541513 : return;
890 : }
891 :
892 6954658 : if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
893 6907755 : send_blob_update(instance);
894 : }
895 :
896 : // we have an active driver for this instance
897 6954658 : bool result = drivers[instance]->read();
898 6954658 : uint32_t tnow = AP_HAL::millis();
899 :
900 : // if we did not get a message, and the idle timer of 2 seconds
901 : // has expired, re-initialise the GPS. This will cause GPS
902 : // detection to run again
903 6954658 : bool new_data_or_timeout = false;
904 6954658 : if (!result) {
905 6243451 : if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
906 9 : memset((void *)&state[instance], 0, sizeof(state[instance]));
907 9 : state[instance].instance = instance;
908 9 : state[instance].hdop = GPS_UNKNOWN_DOP;
909 9 : state[instance].vdop = GPS_UNKNOWN_DOP;
910 9 : timing[instance].last_message_time_ms = tnow;
911 9 : timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
912 9 : update_frame_timing_health(instance);
913 : // do not try to detect again if type is MAV or UAVCAN
914 15 : if (type == GPS_TYPE_MAV ||
915 12 : type == GPS_TYPE_UAVCAN ||
916 21 : type == GPS_TYPE_UAVCAN_RTK_BASE ||
917 6 : type == GPS_TYPE_UAVCAN_RTK_ROVER) {
918 3 : state[instance].status = AP_GPS_FixType::NONE;
919 : } else {
920 : // free the driver before we run the next detection, so we
921 : // don't end up with two allocated at any time
922 6 : delete drivers[instance];
923 6 : drivers[instance] = nullptr;
924 6 : state[instance].status = AP_GPS_FixType::NO_GPS;
925 : }
926 : // log this data as a "flag" that the GPS is no longer
927 : // valid (see PR#8144)
928 9 : new_data_or_timeout = true;
929 : }
930 : } else {
931 711207 : if (state[instance].corrected_timestamp_updated) {
932 : // set the timestamp for this messages based on
933 : // set_uart_timestamp() or per specific transport in backend
934 : // , if available
935 710336 : tnow = state[instance].last_corrected_gps_time_us/1000U;
936 710336 : state[instance].corrected_timestamp_updated = false;
937 : }
938 :
939 : // announce the GPS type once
940 711207 : if (!state[instance].announced_detection) {
941 1093 : state[instance].announced_detection = true;
942 1093 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: detected %s", instance + 1, drivers[instance]->name());
943 : }
944 :
945 : // delta will only be correct after parsing two messages.
946 : //
947 711207 : const int32_t delta_ms = int32_t(tnow - timing[instance].last_message_time_ms);
948 711207 : timing[instance].last_message_time_ms = tnow;
949 711207 : if (delta_ms < 0) {
950 : // time went backwards, so we have no measurement of how long
951 : // this message took to arrive. Leave the health counters
952 : // alone rather than either counting a delayed frame or
953 : // resetting delayed_count and masking a real stall
954 1031 : timing[instance].delta_time_ms = 0;
955 : } else {
956 710176 : timing[instance].delta_time_ms = MIN(delta_ms, UINT16_MAX);
957 710176 : update_frame_timing_health(instance);
958 : }
959 : // if GPS disabled for flight testing then don't update fix timing value
960 711207 : if (state[instance].status >= AP_GPS_FixType::FIX_2D && !_force_disable_gps) {
961 703550 : timing[instance].last_fix_time_ms = tnow;
962 : }
963 :
964 711207 : new_data_or_timeout = true;
965 : }
966 :
967 : #if GPS_MAX_RECEIVERS > 1
968 6954658 : if (drivers[instance] && type == GPS_TYPE_UBLOX_RTK_BASE) {
969 : // see if a moving baseline base has some RTCMv3 data
970 : // which we need to pass along to the rover
971 : const uint8_t *rtcm_data;
972 : uint16_t rtcm_len;
973 22030 : if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
974 0 : for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
975 0 : if (i != instance && params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) {
976 : // pass the data to the rover
977 0 : inject_data(i, rtcm_data, rtcm_len);
978 0 : drivers[instance]->clear_RTCMV3();
979 0 : break;
980 : }
981 : }
982 : }
983 : }
984 : #endif
985 :
986 : #if HAL_LOGGING_ENABLED
987 6954658 : if (new_data_or_timeout && should_log()) {
988 687772 : Write_GPS(instance);
989 : }
990 : #else
991 : (void)new_data_or_timeout;
992 : #endif
993 :
994 : #if AP_RTC_ENABLED
995 6954658 : if (state[instance].status >= AP_GPS_FixType::FIX_3D) {
996 6880003 : const uint64_t now = time_epoch_usec(instance);
997 6880003 : if (now != 0) {
998 6850211 : AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS);
999 : }
1000 : }
1001 : #endif
1002 : }
1003 :
1004 :
1005 : #if GPS_MOVING_BASELINE
1006 0 : bool AP_GPS::get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading)
1007 : {
1008 0 : for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
1009 0 : if (drivers[i] &&
1010 0 : state[i].relposheading_ts != 0 &&
1011 0 : AP_HAL::millis() - state[i].relposheading_ts < 500) {
1012 0 : relPosHeading = state[i].relPosHeading;
1013 0 : relPosLength = state[i].relPosLength;
1014 0 : relPosD = state[i].relPosD;
1015 0 : accHeading = state[i].accHeading;
1016 0 : timestamp = state[i].relposheading_ts;
1017 0 : return true;
1018 : }
1019 : }
1020 0 : return false;
1021 : }
1022 :
1023 0 : bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
1024 : {
1025 0 : for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
1026 0 : if (drivers[i] && params[i].type == GPS_TYPE_UBLOX_RTK_BASE) {
1027 0 : return drivers[i]->get_RTCMV3(bytes, len);
1028 : }
1029 : }
1030 0 : return false;
1031 : }
1032 :
1033 0 : void AP_GPS::clear_RTCMV3()
1034 : {
1035 0 : for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
1036 0 : if (drivers[i] && params[i].type == GPS_TYPE_UBLOX_RTK_BASE) {
1037 0 : drivers[i]->clear_RTCMV3();
1038 : }
1039 : }
1040 0 : }
1041 :
1042 : /*
1043 : inject Moving Baseline Data messages.
1044 : */
1045 0 : void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length)
1046 : {
1047 0 : for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
1048 0 : if (params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) {
1049 : // pass the data to the rover
1050 0 : inject_data(i, data, length);
1051 0 : break;
1052 : }
1053 : }
1054 0 : }
1055 : #endif //#if GPS_MOVING_BASELINE
1056 :
1057 : /*
1058 : update all GPS instances
1059 : */
1060 7456628 : void AP_GPS::update(void)
1061 : {
1062 7456628 : WITH_SEMAPHORE(rsem);
1063 :
1064 22369884 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
1065 14913256 : update_instance(i);
1066 : }
1067 :
1068 : // calculate number of instances
1069 22369884 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
1070 14913256 : if (drivers[i] != nullptr) {
1071 6964540 : num_instances = i+1;
1072 : }
1073 : }
1074 :
1075 : #if GPS_MAX_RECEIVERS > 1
1076 : #if HAL_LOGGING_ENABLED
1077 7456628 : const uint8_t old_primary = primary_instance;
1078 : #endif
1079 7456628 : update_primary();
1080 : #if HAL_LOGGING_ENABLED
1081 7456628 : if (primary_instance != old_primary) {
1082 13 : AP::logger().Write_Event(LogEvent::GPS_PRIMARY_CHANGED);
1083 : }
1084 : #endif // HAL_LOGGING_ENABLED
1085 : #endif // GPS_MAX_RECEIVERS > 1
1086 :
1087 : #ifndef HAL_BUILD_AP_PERIPH
1088 : // update notify with gps status. We always base this on the primary_instance
1089 7456628 : AP_Notify::flags.gps_status = state[primary_instance].status;
1090 7456628 : AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
1091 : #endif
1092 7456628 : }
1093 :
1094 : /*
1095 : update primary GPS instance
1096 : */
1097 : #if GPS_MAX_RECEIVERS > 1
1098 7456628 : void AP_GPS::update_primary(void)
1099 : {
1100 : #if AP_GPS_BLENDED_ENABLED
1101 : /*
1102 : if blending is requested, attempt to calculate weighting for
1103 : each GPS
1104 : we do not do blending if using moving baseline yaw as the rover is
1105 : significant lagged and gives no more information on position or
1106 : velocity
1107 : */
1108 7456628 : const bool using_moving_base = is_rtk_base(0) || is_rtk_base(1);
1109 7456628 : if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::BLEND && !using_moving_base) {
1110 17961 : _output_is_blended = ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_weights();
1111 : } else {
1112 7438667 : _output_is_blended = false;
1113 7438667 : ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->zero_health_counter();
1114 : }
1115 :
1116 7456628 : if (_output_is_blended) {
1117 : // Use the weighting to calculate blended GPS states
1118 16024 : ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_state();
1119 : // set primary to the virtual instance
1120 16024 : primary_instance = GPS_BLENDED_INSTANCE;
1121 16024 : return;
1122 : }
1123 : #endif // AP_GPS_BLENDED_ENABLED
1124 :
1125 : // check the primary param is set to possible GPS
1126 7440604 : int8_t primary_param = _primary.get();
1127 7440604 : if ((primary_param < 0) || (primary_param>=GPS_MAX_RECEIVERS)) {
1128 0 : primary_param = 0;
1129 : }
1130 : // if primary is not enabled try first instance
1131 7440604 : if (get_type(primary_param) == GPS_TYPE_NONE) {
1132 33857 : primary_param = 0;
1133 : }
1134 :
1135 7440604 : if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::NONE) {
1136 : // No switching of GPSs, always use the primary instance
1137 0 : primary_instance = primary_param;
1138 0 : return;
1139 : }
1140 :
1141 7440604 : uint32_t now = AP_HAL::millis();
1142 :
1143 : // special handling of RTK moving baseline pair. Always use the
1144 : // base as the rover position is derived from the base, which
1145 : // means the rover always has worse position and velocity than the
1146 : // base. This overrides the normal logic which would select the
1147 : // rover as it typically is in fix type 6 (RTK) whereas base is
1148 : // usually fix type 3
1149 22270520 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
1150 14881208 : if (is_rtk_base(i) &&
1151 14881208 : is_rtk_rover(i^1) &&
1152 25646 : ((state[i].status >= AP_GPS_FixType::FIX_3D) || (state[i].status >= state[i^1].status))) {
1153 25646 : if (primary_instance != i) {
1154 0 : _last_instance_swap_ms = now;
1155 0 : primary_instance = i;
1156 : }
1157 : // don't do any more switching logic. We don't want the
1158 : // RTK status of the rover to cause a switch
1159 25646 : return;
1160 : }
1161 : }
1162 :
1163 : #if AP_GPS_BLENDED_ENABLED
1164 : // handling switching away from blended GPS
1165 7414958 : if (primary_instance == GPS_BLENDED_INSTANCE) {
1166 4 : primary_instance = 0;
1167 8 : for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
1168 : // choose GPS with highest state or higher number of
1169 : // satellites. Reject a GPS with an old update time, as it
1170 : // may be the old timestamp that triggered the loss of
1171 : // blending
1172 4 : const uint32_t delay_threshold = 400;
1173 4 : const bool higher_status = state[i].status > state[primary_instance].status;
1174 4 : const bool old_data_primary = (now - state[primary_instance].last_gps_time_ms) > delay_threshold;
1175 4 : const bool old_data = (now - state[i].last_gps_time_ms) > delay_threshold;
1176 4 : const bool equal_status = state[i].status == state[primary_instance].status;
1177 4 : const bool more_sats = state[i].num_sats > state[primary_instance].num_sats;
1178 4 : if (old_data && !old_data_primary) {
1179 : // don't switch to a GPS that has not updated in 400ms
1180 0 : continue;
1181 : }
1182 4 : if (state[i].status < AP_GPS_FixType::FIX_3D) {
1183 : // don't use a GPS without 3D fix
1184 4 : continue;
1185 : }
1186 : // select the new GPS if the primary has old data, or new
1187 : // GPS either has higher status, or has the same status
1188 : // and more satellites
1189 0 : if ((old_data_primary && !old_data) ||
1190 0 : higher_status ||
1191 0 : (equal_status && more_sats)) {
1192 0 : primary_instance = i;
1193 : }
1194 : }
1195 4 : _last_instance_swap_ms = now;
1196 4 : return;
1197 : }
1198 : #endif // AP_GPS_BLENDED_ENABLED
1199 :
1200 : // Use primary if 3D fix or better
1201 7414954 : if (((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_PRIMARY_IF_3D_FIX) && (state[primary_param].status >= AP_GPS_FixType::FIX_3D)) {
1202 : // Primary GPS has a least a 3D fix, switch to it if necessary
1203 0 : if (primary_instance != primary_param) {
1204 0 : primary_instance = primary_param;
1205 0 : _last_instance_swap_ms = now;
1206 : }
1207 0 : return;
1208 : }
1209 :
1210 : // handle switch between real GPSs
1211 22244862 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
1212 14829908 : if (i == primary_instance) {
1213 7414952 : continue;
1214 : }
1215 7414956 : if (state[i].status > state[primary_instance].status) {
1216 : // we have a higher status lock, or primary is set to the blended GPS, change GPS
1217 5 : primary_instance = i;
1218 5 : _last_instance_swap_ms = now;
1219 5 : continue;
1220 : }
1221 :
1222 7414951 : bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
1223 :
1224 7414951 : if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
1225 :
1226 0 : bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
1227 :
1228 0 : if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
1229 0 : (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) {
1230 : // this GPS has more satellites than the
1231 : // current primary, switch primary. Once we switch we will
1232 : // then tend to stick to the new GPS as primary. We don't
1233 : // want to switch too often as it will look like a
1234 : // position shift to the controllers.
1235 0 : primary_instance = i;
1236 0 : _last_instance_swap_ms = now;
1237 : }
1238 : }
1239 : }
1240 : }
1241 : #endif // GPS_MAX_RECEIVERS > 1
1242 :
1243 : #if HAL_GCS_ENABLED
1244 0 : void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
1245 : {
1246 : mavlink_gps_inject_data_t packet;
1247 0 : mavlink_msg_gps_inject_data_decode(&msg, &packet);
1248 :
1249 0 : if (packet.len > sizeof(packet.data)) {
1250 : // invalid packet
1251 0 : return;
1252 : }
1253 :
1254 0 : handle_gps_rtcm_fragment(0, packet.data, packet.len);
1255 : }
1256 :
1257 : /*
1258 : pass along a mavlink message (for MAV type)
1259 : */
1260 915 : void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
1261 : {
1262 915 : switch (msg.msgid) {
1263 0 : case MAVLINK_MSG_ID_GPS_RTCM_DATA:
1264 : // pass data to de-fragmenter
1265 0 : handle_gps_rtcm_data(chan, msg);
1266 0 : break;
1267 0 : case MAVLINK_MSG_ID_GPS_INJECT_DATA:
1268 0 : handle_gps_inject(msg);
1269 0 : break;
1270 915 : default: {
1271 : uint8_t i;
1272 1830 : for (i=0; i<num_instances; i++) {
1273 915 : if ((drivers[i] != nullptr) && (params[i].type != GPS_TYPE_NONE)) {
1274 915 : drivers[i]->handle_msg(msg);
1275 : }
1276 : }
1277 915 : break;
1278 : }
1279 : }
1280 915 : }
1281 : #endif
1282 :
1283 : #if HAL_MSP_GPS_ENABLED
1284 76 : void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt)
1285 : {
1286 140 : for (uint8_t i=0; i<num_instances; i++) {
1287 64 : if (drivers[i] != nullptr && params[i].type == GPS_TYPE_MSP) {
1288 64 : drivers[i]->handle_msp(pkt);
1289 : }
1290 : }
1291 76 : }
1292 : #endif // HAL_MSP_GPS_ENABLED
1293 :
1294 : #if AP_EXTERNAL_AHRS_ENABLED
1295 :
1296 14409 : bool AP_GPS::get_first_external_instance(uint8_t& instance) const
1297 : {
1298 14635 : for (uint8_t i=0; i<num_instances; i++) {
1299 10849 : if (drivers[i] != nullptr && params[i].type == GPS_TYPE_EXTERNAL_AHRS) {
1300 10623 : instance = i;
1301 10623 : return true;
1302 : }
1303 : }
1304 3786 : return false;
1305 : }
1306 :
1307 26587 : void AP_GPS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt, const uint8_t instance)
1308 : {
1309 26587 : if (get_type(instance) == GPS_TYPE_EXTERNAL_AHRS && drivers[instance] != nullptr) {
1310 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1311 15139 : if (pkt.longitude == 0 && pkt.latitude == 0) {
1312 0 : AP_HAL::panic("Invalid location passed to AP_GPS::handle_external");
1313 : }
1314 : #endif
1315 15139 : drivers[instance]->handle_external(pkt);
1316 : }
1317 26587 : }
1318 : #endif // AP_EXTERNAL_AHRS_ENABLED
1319 :
1320 : /**
1321 : Lock a GPS port, preventing the GPS driver from using it. This can
1322 : be used to allow a user to control a GPS port via the
1323 : SERIAL_CONTROL protocol
1324 : */
1325 0 : void AP_GPS::lock_port(uint8_t instance, bool lock)
1326 : {
1327 :
1328 0 : if (instance >= GPS_MAX_RECEIVERS) {
1329 0 : return;
1330 : }
1331 0 : if (lock) {
1332 0 : locked_ports |= (1U<<instance);
1333 : } else {
1334 0 : locked_ports &= ~(1U<<instance);
1335 : }
1336 : }
1337 :
1338 : // Inject a packet of raw binary to a GPS
1339 0 : void AP_GPS::inject_data(const uint8_t *data, uint16_t len)
1340 : {
1341 : //Support broadcasting to all GPSes.
1342 0 : if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
1343 0 : for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
1344 0 : if (is_rtk_rover(i)) {
1345 : // we don't externally inject to moving baseline rover
1346 0 : continue;
1347 : }
1348 0 : inject_data(i, data, len);
1349 : }
1350 : } else {
1351 0 : inject_data(_inject_to, data, len);
1352 : }
1353 0 : }
1354 :
1355 0 : void AP_GPS::inject_data(uint8_t instance, const uint8_t *data, uint16_t len)
1356 : {
1357 0 : if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
1358 0 : drivers[instance]->inject_data(data, len);
1359 : }
1360 0 : }
1361 :
1362 : /*
1363 : get GPS yaw following mavlink GPS_RAW_INT and GPS2_RAW
1364 : convention. We return 0 if the GPS is not configured to provide
1365 : yaw. We return 65535 for a GPS configured to provide yaw that can't
1366 : currently provide it. We return from 1 to 36000 for yaw otherwise
1367 : */
1368 1121958 : uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
1369 : {
1370 1121958 : if (!have_gps_yaw_configured(instance)) {
1371 1119218 : return 0;
1372 : }
1373 : float yaw_deg, accuracy_deg;
1374 : uint32_t time_ms;
1375 2740 : if (!gps_yaw_deg(instance, yaw_deg, accuracy_deg, time_ms)) {
1376 58 : return 65535;
1377 : }
1378 2682 : int yaw_cd = wrap_360_cd(yaw_deg * 100);
1379 2682 : if (yaw_cd == 0) {
1380 0 : return 36000;
1381 : }
1382 2682 : return yaw_cd;
1383 : }
1384 :
1385 : #if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
1386 1117165 : void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
1387 : {
1388 : // Only send if enabled
1389 1117165 : if (get_type(0) == GPS_TYPE_NONE) {
1390 3930 : return;
1391 : }
1392 :
1393 1113235 : const Location &loc = location(0);
1394 1113235 : float hacc = 0.0f;
1395 1113235 : float vacc = 0.0f;
1396 1113235 : float sacc = 0.0f;
1397 1113235 : float undulation = 0.0;
1398 1113235 : int32_t height_elipsoid_mm = 0;
1399 1113235 : if (get_undulation(0, undulation)) {
1400 1033482 : height_elipsoid_mm = loc.alt*10 - undulation*1000;
1401 : }
1402 1113235 : horizontal_accuracy(0, hacc);
1403 1113235 : vertical_accuracy(0, vacc);
1404 1113235 : speed_accuracy(0, sacc);
1405 8905880 : mavlink_msg_gps_raw_int_send(
1406 : chan,
1407 1113235 : last_fix_time_ms(0)*(uint64_t)1000,
1408 1113235 : (uint8_t)status(0),
1409 1113235 : loc.lat, // in 1E7 degrees
1410 1113235 : loc.lng, // in 1E7 degrees
1411 1113235 : loc.alt * 10UL, // in mm
1412 1113235 : get_hdop(0),
1413 1113235 : get_vdop(0),
1414 1113235 : ground_speed(0)*100, // cm/s
1415 1113235 : ground_course(0)*100, // 1/100 degrees,
1416 1113235 : num_sats(0),
1417 : height_elipsoid_mm, // Ellipsoid height in mm
1418 1113235 : hacc * 1000, // one-sigma standard deviation in mm
1419 1113235 : vacc * 1000, // one-sigma standard deviation in mm
1420 1113235 : sacc * 1000, // one-sigma standard deviation in mm/s
1421 : 0, // TODO one-sigma heading accuracy standard deviation
1422 1113235 : gps_yaw_cdeg(0));
1423 : }
1424 : #endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED
1425 :
1426 : #if AP_GPS_GPS2_RAW_SENDING_ENABLED
1427 1117087 : void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
1428 : {
1429 : // always send the message if 2nd GPS is configured
1430 1117087 : if (params[1].type == GPS_TYPE_NONE) {
1431 1108364 : return;
1432 : }
1433 :
1434 8723 : const Location &loc = location(1);
1435 8723 : float hacc = 0.0f;
1436 8723 : float vacc = 0.0f;
1437 8723 : float sacc = 0.0f;
1438 8723 : float undulation = 0.0;
1439 8723 : float height_elipsoid_mm = 0;
1440 8723 : if (get_undulation(1, undulation)) {
1441 7764 : height_elipsoid_mm = loc.alt*10 - undulation*1000;
1442 : }
1443 8723 : horizontal_accuracy(1, hacc);
1444 8723 : vertical_accuracy(1, vacc);
1445 8723 : speed_accuracy(1, sacc);
1446 78507 : mavlink_msg_gps2_raw_send(
1447 : chan,
1448 8723 : last_fix_time_ms(1)*(uint64_t)1000,
1449 8723 : (uint8_t)status(1),
1450 8723 : loc.lat,
1451 8723 : loc.lng,
1452 8723 : loc.alt * 10UL,
1453 8723 : get_hdop(1),
1454 8723 : get_vdop(1),
1455 8723 : ground_speed(1)*100, // cm/s
1456 8723 : ground_course(1)*100, // 1/100 degrees,
1457 8723 : num_sats(1),
1458 8723 : state[1].rtk_num_sats,
1459 : state[1].rtk_age_ms,
1460 8723 : gps_yaw_cdeg(1),
1461 : height_elipsoid_mm, // Ellipsoid height in mm
1462 8723 : hacc * 1000, // one-sigma standard deviation in mm
1463 8723 : vacc * 1000, // one-sigma standard deviation in mm
1464 8723 : sacc * 1000, // one-sigma standard deviation in mm/s
1465 : 0); // TODO one-sigma heading accuracy standard deviation
1466 : }
1467 : #endif // AP_GPS_GPS2_RAW_SENDING_ENABLED
1468 :
1469 : #if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
1470 2234174 : void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
1471 : {
1472 2234174 : if (inst >= GPS_MAX_RECEIVERS) {
1473 0 : return;
1474 : }
1475 2234174 : if (drivers[inst] != nullptr && drivers[inst]->supports_mavlink_gps_rtk_message()) {
1476 129 : drivers[inst]->send_mavlink_gps_rtk(chan);
1477 : }
1478 : }
1479 : #endif
1480 :
1481 41313222 : bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const
1482 : {
1483 119260182 : for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
1484 80292312 : if (params[i].type != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) {
1485 2345352 : instance = i;
1486 2345352 : return true;
1487 : }
1488 : }
1489 38967870 : return false;
1490 : }
1491 :
1492 1 : void AP_GPS::broadcast_first_configuration_failure_reason(void) const
1493 : {
1494 : uint8_t unconfigured;
1495 1 : if (first_unconfigured_gps(unconfigured)) {
1496 1 : if (drivers[unconfigured] == nullptr) {
1497 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1);
1498 : } else {
1499 0 : drivers[unconfigured]->broadcast_configuration_failure_reason();
1500 : }
1501 : }
1502 1 : }
1503 :
1504 : // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
1505 28761 : bool AP_GPS::all_consistent(float &distance) const
1506 : {
1507 : // return true immediately if only one valid receiver
1508 57786 : if (num_instances <= 1 ||
1509 28761 : drivers[0] == nullptr || params[0].type == GPS_TYPE_NONE) {
1510 28497 : distance = 0;
1511 28497 : return true;
1512 : }
1513 :
1514 : // calculate distance
1515 264 : distance = state[0].location.get_distance_NED(state[1].location).length();
1516 : // success if distance is within 50m
1517 264 : return (distance < 50);
1518 : }
1519 :
1520 : /*
1521 : re-assemble fragmented RTCM data
1522 : */
1523 0 : void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len)
1524 : {
1525 0 : if ((flags & 1) == 0) {
1526 : // it is not fragmented, pass direct
1527 0 : inject_data(data, len);
1528 0 : return;
1529 : }
1530 :
1531 : // see if we need to allocate re-assembly buffer
1532 0 : if (rtcm_buffer == nullptr) {
1533 0 : rtcm_buffer = (struct rtcm_buffer *)calloc(1, sizeof(*rtcm_buffer));
1534 0 : if (rtcm_buffer == nullptr) {
1535 : // nothing to do but discard the data
1536 0 : return;
1537 : }
1538 : }
1539 :
1540 0 : const uint8_t fragment = (flags >> 1U) & 0x03;
1541 0 : const uint8_t sequence = (flags >> 3U) & 0x1F;
1542 0 : uint8_t* start_of_fragment_in_buffer = &rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN * (uint16_t)fragment];
1543 0 : bool should_clear_previous_fragments = false;
1544 :
1545 0 : if (rtcm_buffer->fragments_received) {
1546 0 : const bool sequence_nr_changed = rtcm_buffer->sequence != sequence;
1547 0 : const bool seen_this_fragment_index = rtcm_buffer->fragments_received & (1U << fragment);
1548 :
1549 : // check whether this is a duplicate fragment. If it is, we can
1550 : // return early.
1551 0 : if (!sequence_nr_changed && seen_this_fragment_index && !memcmp(start_of_fragment_in_buffer, data, len)) {
1552 0 : return;
1553 : }
1554 :
1555 : // not a duplicate
1556 0 : should_clear_previous_fragments = sequence_nr_changed || seen_this_fragment_index;
1557 : }
1558 :
1559 0 : if (should_clear_previous_fragments) {
1560 : // we have one or more partial fragments already received
1561 : // which conflict with the new fragment, discard previous fragments
1562 0 : rtcm_buffer->fragment_count = 0;
1563 0 : rtcm_stats.fragments_discarded += __builtin_popcount(rtcm_buffer->fragments_received);
1564 0 : rtcm_buffer->fragments_received = 0;
1565 : }
1566 :
1567 : // add this fragment
1568 0 : rtcm_buffer->sequence = sequence;
1569 0 : rtcm_buffer->fragments_received |= (1U << fragment);
1570 :
1571 : // copy the data
1572 0 : memcpy(start_of_fragment_in_buffer, data, len);
1573 :
1574 : // when we get a fragment of less than max size then we know the
1575 : // number of fragments. Note that this means if you want to send a
1576 : // block of RTCM data of an exact multiple of the buffer size you
1577 : // need to send a final packet of zero length
1578 0 : if (len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
1579 0 : rtcm_buffer->fragment_count = fragment+1;
1580 0 : rtcm_buffer->total_length = (MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*fragment) + len;
1581 0 : } else if (rtcm_buffer->fragments_received == 0x0F) {
1582 : // special case of 4 full fragments
1583 0 : rtcm_buffer->fragment_count = 4;
1584 0 : rtcm_buffer->total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*4;
1585 : }
1586 :
1587 :
1588 : // see if we have all fragments
1589 0 : if (rtcm_buffer->fragment_count != 0 &&
1590 0 : rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) {
1591 : // we have them all, inject
1592 0 : rtcm_stats.fragments_used += __builtin_popcount(rtcm_buffer->fragments_received);
1593 0 : inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length);
1594 0 : rtcm_buffer->fragment_count = 0;
1595 0 : rtcm_buffer->fragments_received = 0;
1596 : }
1597 : }
1598 :
1599 : /*
1600 : re-assemble GPS_RTCM_DATA message
1601 : */
1602 0 : void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_t &msg)
1603 : {
1604 : mavlink_gps_rtcm_data_t packet;
1605 0 : mavlink_msg_gps_rtcm_data_decode(&msg, &packet);
1606 :
1607 0 : if (packet.len > sizeof(packet.data)) {
1608 : // invalid packet
1609 0 : return;
1610 : }
1611 :
1612 : #if AP_GPS_RTCM_DECODE_ENABLED
1613 0 : if (!option_set(DriverOptions::DisableRTCMDecode)) {
1614 0 : const uint16_t mask = (1U << unsigned(chan));
1615 0 : rtcm.seen_mav_channels |= mask;
1616 0 : if (option_set(DriverOptions::AlwaysRTCMDecode) ||
1617 0 : (rtcm.seen_mav_channels & ~mask) != 0) {
1618 : /*
1619 : we are seeing RTCM on multiple mavlink channels. We will run
1620 : the data through a full per-channel RTCM decoder
1621 : */
1622 0 : if (parse_rtcm_injection(chan, packet)) {
1623 0 : return;
1624 : }
1625 : }
1626 : }
1627 : #endif
1628 :
1629 0 : handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len);
1630 : }
1631 :
1632 : #if AP_GPS_RTCM_DECODE_ENABLED
1633 : /*
1634 : fully parse RTCM data coming in from a MAVLink channel, when we have
1635 : a full message inject it to the GPS. This approach allows for 2 or
1636 : more MAVLink channels to be used for the same RTCM data, allowing
1637 : for redundent transports for maximum reliability at the cost of some
1638 : extra CPU and a bit of re-assembly lag
1639 : */
1640 0 : bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt)
1641 : {
1642 0 : if (static_cast<int>(chan) >= MAVLINK_COMM_NUM_BUFFERS) {
1643 0 : return false;
1644 : }
1645 0 : if (rtcm.parsers[chan] == nullptr) {
1646 0 : rtcm.parsers[chan] = NEW_NOTHROW RTCM3_Parser();
1647 0 : if (rtcm.parsers[chan] == nullptr) {
1648 0 : return false;
1649 : }
1650 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: RTCM parsing for chan %u", unsigned(chan));
1651 : }
1652 0 : for (uint16_t i=0; i<pkt.len; i++) {
1653 0 : if (rtcm.parsers[chan]->read(pkt.data[i])) {
1654 : // we have a full message, inject it
1655 0 : const uint8_t *buf = nullptr;
1656 0 : uint16_t len = rtcm.parsers[chan]->get_len(buf);
1657 :
1658 : // see if we have already sent it. This prevents
1659 : // duplicates from multiple sources
1660 0 : const uint32_t crc = crc_crc32(0, buf, len);
1661 :
1662 : #if HAL_LOGGING_ENABLED
1663 : // @LoggerMessage: RTCM
1664 : // @Description: GPS atmospheric perturbation data
1665 : // @Field: TimeUS: Time since system startup
1666 : // @Field: Chan: mavlink channel number this data was received on
1667 : // @Field: RTCMId: ID field from RTCM packet
1668 : // @Field: Len: RTCM packet length
1669 : // @Field: CRC: calculated crc32 for the packet
1670 0 : AP::logger().WriteStreaming("RTCM", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI",
1671 : AP_HAL::micros64(),
1672 : uint8_t(chan),
1673 0 : rtcm.parsers[chan]->get_id(),
1674 : len,
1675 : crc);
1676 : #endif
1677 :
1678 0 : bool already_seen = false;
1679 0 : for (uint8_t c=0; c<ARRAY_SIZE(rtcm.sent_crc); c++) {
1680 0 : if (rtcm.sent_crc[c] == crc) {
1681 : // we have already sent this message
1682 0 : already_seen = true;
1683 0 : break;
1684 : }
1685 : }
1686 0 : if (already_seen) {
1687 0 : continue;
1688 : }
1689 0 : rtcm.sent_crc[rtcm.sent_idx] = crc;
1690 0 : rtcm.sent_idx = (rtcm.sent_idx+1) % ARRAY_SIZE(rtcm.sent_crc);
1691 :
1692 0 : if (buf != nullptr && len > 0) {
1693 0 : inject_data(buf, len);
1694 : }
1695 0 : rtcm.parsers[chan]->reset();
1696 : }
1697 : }
1698 0 : return true;
1699 : }
1700 : #endif // AP_GPS_RTCM_DECODE_ENABLED
1701 :
1702 : #if HAL_LOGGING_ENABLED
1703 5184 : void AP_GPS::Write_AP_Logger_Log_Startup_messages()
1704 : {
1705 7030 : for (uint8_t instance=0; instance<num_instances; instance++) {
1706 1846 : if (drivers[instance] == nullptr || state[instance].status == AP_GPS_FixType::NO_GPS) {
1707 1 : continue;
1708 : }
1709 1845 : drivers[instance]->Write_AP_Logger_Log_Startup_messages();
1710 : }
1711 5184 : }
1712 : #endif
1713 :
1714 : /*
1715 : return the expected lag (in seconds) in the position and velocity readings from the gps
1716 : return true if the GPS hardware configuration is known or the delay parameter has been set
1717 : */
1718 163053392 : bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
1719 : {
1720 : // always ensure a lag is provided
1721 163053392 : lag_sec = 0.1f;
1722 :
1723 163053392 : if (instance >= GPS_MAX_INSTANCES) {
1724 0 : return false;
1725 : }
1726 :
1727 : #if AP_GPS_BLENDED_ENABLED
1728 : // return lag of blended GPS
1729 163053392 : if (instance == GPS_BLENDED_INSTANCE) {
1730 41284477 : return drivers[instance]->get_lag(lag_sec);
1731 : }
1732 : #endif
1733 :
1734 121768915 : if (params[instance].delay_ms > 0) {
1735 : // if the user has specified a non zero time delay, always return that value
1736 0 : lag_sec = 0.001f * (float)params[instance].delay_ms;
1737 : // the user is always right !!
1738 0 : return true;
1739 121768915 : } else if (drivers[instance] == nullptr || state[instance].status == AP_GPS_FixType::NO_GPS) {
1740 : // no GPS was detected in this instance so return the worst possible lag term
1741 43377052 : const auto type = params[instance].type;
1742 43377052 : if (type == GPS_TYPE_NONE) {
1743 40979176 : lag_sec = 0.0f;
1744 40979176 : return true;
1745 : }
1746 2397876 : return type == GPS_TYPE_AUTO;
1747 : } else {
1748 : // the user has not specified a delay so we determine it from the GPS type
1749 78391863 : return drivers[instance]->get_lag(lag_sec);
1750 : }
1751 : }
1752 :
1753 : // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
1754 123468855 : const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
1755 : {
1756 123468855 : if (instance >= GPS_MAX_INSTANCES) {
1757 : // we have to return a reference so use instance 0
1758 0 : return params[0].antenna_offset;
1759 : }
1760 :
1761 : #if AP_GPS_BLENDED_ENABLED
1762 123468855 : if (instance == GPS_BLENDED_INSTANCE) {
1763 : // return an offset for the blended GPS solution
1764 41156285 : return ((AP_GPS_Blended*)drivers[instance])->get_antenna_offset();
1765 : }
1766 : #endif
1767 :
1768 82312570 : return params[instance].antenna_offset;
1769 : }
1770 :
1771 : /*
1772 : returns the desired gps update rate in milliseconds
1773 : this does not provide any guarantee that the GPS is updating at the requested
1774 : rate it is simply a helper for use in the backends for determining what rate
1775 : they should be configuring the GPS to run at
1776 : */
1777 33735 : uint16_t AP_GPS::get_rate_ms(uint8_t instance) const
1778 : {
1779 : // sanity check
1780 33735 : if (instance >= num_instances || params[instance].rate_ms <= 0) {
1781 0 : return GPS_MAX_RATE_MS;
1782 : }
1783 33735 : return MIN(params[instance].rate_ms, GPS_MAX_RATE_MS);
1784 : }
1785 :
1786 1152734 : bool AP_GPS::is_healthy(uint8_t instance) const
1787 : {
1788 1152734 : if (instance >= GPS_MAX_INSTANCES) {
1789 0 : return false;
1790 : }
1791 :
1792 1152734 : if (get_type(_primary.get()) == GPS_TYPE_NONE) {
1793 3824 : return false;
1794 : }
1795 :
1796 : #ifndef HAL_BUILD_AP_PERIPH
1797 : /*
1798 : on AP_Periph handling of timing is done by the flight controller
1799 : receiving the DroneCAN messages
1800 : */
1801 : /*
1802 : allow two lost frames before declaring the GPS unhealthy, but
1803 : require the average frame rate to be close to 5Hz.
1804 :
1805 : We allow for a rate of 3Hz average for a moving baseline rover
1806 : due to the packet loss that happens with the RTCMv3 data and the
1807 : fact that the rate of yaw data is not critical
1808 : */
1809 1148910 : const uint8_t delay_threshold = 2;
1810 1148910 : const float delay_avg_max = is_rtk_rover(instance) ? 333 : 215;
1811 1148910 : const GPS_timing &t = timing[instance];
1812 3445795 : bool delay_ok = (t.delayed_count < delay_threshold) &&
1813 2296674 : t.average_delta_ms < delay_avg_max &&
1814 1147764 : state[instance].lagged_sample_count < 5;
1815 1148910 : if (!delay_ok) {
1816 3266 : return false;
1817 : }
1818 : #endif // HAL_BUILD_AP_PERIPH
1819 :
1820 2223513 : return drivers[instance] != nullptr &&
1821 2223513 : drivers[instance]->is_healthy();
1822 : }
1823 :
1824 983 : bool AP_GPS::prepare_for_arming(void) {
1825 983 : bool all_passed = true;
1826 2949 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1827 1966 : if (drivers[i] != nullptr) {
1828 990 : all_passed &= drivers[i]->prepare_for_arming();
1829 : }
1830 : }
1831 983 : return all_passed;
1832 : }
1833 :
1834 38456 : bool AP_GPS::pre_arm_checks(char failure_msg[], uint16_t failure_msg_len)
1835 : {
1836 : // the DroneCAN class has additional checks for DroneCAN-specific
1837 : // parameters:
1838 : #if AP_GPS_DRONECAN_ENABLED
1839 38456 : if (!AP_GPS_DroneCAN::inter_instance_pre_arm_checks(failure_msg, failure_msg_len)) {
1840 3 : return false;
1841 : }
1842 : #endif
1843 :
1844 115353 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1845 76906 : if (is_rtk_rover(i)) {
1846 71 : if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) {
1847 6 : hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1));
1848 6 : return false;
1849 : }
1850 : }
1851 : }
1852 :
1853 : #if AP_GPS_BLENDED_ENABLED
1854 38447 : if (!drivers[GPS_BLENDED_INSTANCE]->is_healthy()) {
1855 22 : hal.util->snprintf(failure_msg, failure_msg_len, "GPS blending unhealthy");
1856 22 : return false;
1857 : }
1858 : #endif
1859 :
1860 38425 : return true;
1861 : }
1862 :
1863 1121120 : bool AP_GPS::logging_failed(void) const {
1864 1121120 : if (!logging_enabled()) {
1865 1121120 : return false;
1866 : }
1867 :
1868 0 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1869 0 : if ((drivers[i] != nullptr) && !(drivers[i]->logging_healthy())) {
1870 0 : return true;
1871 : }
1872 : }
1873 :
1874 0 : return false;
1875 : }
1876 :
1877 : // get iTOW, if supported, zero otherwie
1878 0 : uint32_t AP_GPS::get_itow(uint8_t instance) const
1879 : {
1880 0 : if (instance >= GPS_MAX_RECEIVERS || drivers[instance] == nullptr) {
1881 0 : return 0;
1882 : }
1883 0 : return drivers[instance]->get_last_itow_ms();
1884 : }
1885 :
1886 0 : bool AP_GPS::get_error_codes(uint8_t instance, uint32_t &error_codes) const
1887 : {
1888 0 : if (instance >= GPS_MAX_RECEIVERS || drivers[instance] == nullptr) {
1889 0 : return false;
1890 : }
1891 :
1892 0 : return drivers[instance]->get_error_codes(error_codes);
1893 : }
1894 :
1895 : // get the difference between WGS84 and AMSL. A positive value means
1896 : // the AMSL height is higher than WGS84 ellipsoid height
1897 1811347 : bool AP_GPS::get_undulation(uint8_t instance, float &undulation) const
1898 : {
1899 1811347 : if (!state[instance].have_undulation) {
1900 96860 : return false;
1901 : }
1902 1714487 : undulation = state[instance].undulation;
1903 1714487 : return true;
1904 : }
1905 :
1906 : #if HAL_LOGGING_ENABLED
1907 : // Logging support:
1908 : // Write an GPS packet
1909 689376 : void AP_GPS::Write_GPS(uint8_t i)
1910 : {
1911 689376 : const uint64_t time_us = AP_HAL::micros64();
1912 689376 : const Location &loc = location(i);
1913 :
1914 689376 : float yaw_deg=0, yaw_accuracy_deg=0;
1915 : uint32_t yaw_time_ms;
1916 689376 : gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg, yaw_time_ms);
1917 :
1918 689376 : const struct log_GPS pkt {
1919 : LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
1920 : time_us : time_us,
1921 : instance : i,
1922 689376 : status : (uint8_t)status(i),
1923 689376 : gps_week_ms : time_week_ms(i),
1924 689376 : gps_week : time_week(i),
1925 689376 : num_sats : num_sats(i),
1926 689376 : hdop : get_hdop(i),
1927 689376 : latitude : loc.lat,
1928 689376 : longitude : loc.lng,
1929 689376 : altitude : loc.alt,
1930 689376 : ground_speed : ground_speed(i),
1931 689376 : ground_course : ground_course(i),
1932 1378752 : vel_z : velocity(i).z,
1933 : yaw : yaw_deg,
1934 689376 : used : (uint8_t)(AP::gps().primary_sensor() == i)
1935 689376 : };
1936 689376 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
1937 :
1938 : /* write auxiliary accuracy information as well */
1939 689376 : float hacc = 0, vacc = 0, sacc = 0;
1940 689376 : int32_t alt_ellipsoid = INT32_MIN;
1941 689376 : float undulation = 0;
1942 689376 : horizontal_accuracy(i, hacc);
1943 689376 : vertical_accuracy(i, vacc);
1944 689376 : speed_accuracy(i, sacc);
1945 689376 : if (get_undulation(i, undulation)) {
1946 673228 : alt_ellipsoid = loc.alt - (undulation*100);
1947 : }
1948 689376 : struct log_GPA pkt2{
1949 : LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
1950 : time_us : time_us,
1951 : instance : i,
1952 689376 : vdop : get_vdop(i),
1953 1378752 : hacc : (uint16_t)MIN((hacc*100), UINT16_MAX),
1954 1378752 : vacc : (uint16_t)MIN((vacc*100), UINT16_MAX),
1955 1378752 : sacc : (uint16_t)MIN((sacc*100), UINT16_MAX),
1956 : yaw_accuracy : yaw_accuracy_deg,
1957 1378752 : have_vv : (uint8_t)have_vertical_velocity(i),
1958 689376 : sample_ms : last_message_time_ms(i),
1959 689376 : delta_ms : last_message_delta_time_ms(i),
1960 : alt_ellipsoid : alt_ellipsoid,
1961 689376 : rtcm_fragments_used: rtcm_stats.fragments_used,
1962 689376 : rtcm_fragments_discarded: rtcm_stats.fragments_discarded
1963 689376 : };
1964 689376 : AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
1965 689376 : }
1966 : #endif
1967 :
1968 277372998 : bool AP_GPS::is_rtk_base(uint8_t instance) const
1969 : {
1970 277372998 : switch (get_type(instance)) {
1971 455632 : case GPS_TYPE_UBLOX_RTK_BASE:
1972 : case GPS_TYPE_UAVCAN_RTK_BASE:
1973 455632 : return true;
1974 276917366 : default:
1975 276917366 : break;
1976 : }
1977 276917366 : return false;
1978 : }
1979 :
1980 1655802 : bool AP_GPS::is_rtk_rover(uint8_t instance) const
1981 : {
1982 1655802 : switch (get_type(instance)) {
1983 430107 : case GPS_TYPE_UBLOX_RTK_ROVER:
1984 : case GPS_TYPE_UAVCAN_RTK_ROVER:
1985 430107 : return true;
1986 1225695 : default:
1987 1225695 : break;
1988 : }
1989 1225695 : return false;
1990 : }
1991 :
1992 : /*
1993 : return the instance whose moving baseline rover solution provides the yaw
1994 : reported for this instance by gps_yaw_deg
1995 : */
1996 247629826 : uint8_t AP_GPS::yaw_source_instance(uint8_t instance) const
1997 : {
1998 : #if GPS_MAX_RECEIVERS > 1
1999 247629826 : if (is_rtk_base(instance) && is_rtk_rover(instance^1)) {
2000 : // the yaw for a base is provided by its paired rover
2001 404334 : instance ^= 1;
2002 : }
2003 : #endif
2004 247629826 : return instance;
2005 : }
2006 :
2007 : /*
2008 : get GPS based yaw
2009 : */
2010 124160971 : bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const
2011 : {
2012 : // the yaw for a base is reported from its paired rover
2013 124160971 : instance = yaw_source_instance(instance);
2014 124160971 : if (!have_gps_yaw(instance)) {
2015 123793717 : return false;
2016 : }
2017 367254 : yaw_deg = state[instance].gps_yaw;
2018 :
2019 : // get lagged timestamp
2020 367254 : time_ms = state[instance].gps_yaw_time_ms;
2021 : float lag_s;
2022 367254 : if (get_lag(instance, lag_s)) {
2023 367254 : uint32_t lag_ms = lag_s * 1000;
2024 367254 : time_ms -= lag_ms;
2025 : }
2026 :
2027 367254 : if (state[instance].have_gps_yaw_accuracy) {
2028 348349 : accuracy_deg = state[instance].gps_yaw_accuracy;
2029 : } else {
2030 : // fall back to 10 degrees as a generic default
2031 18905 : accuracy_deg = 10;
2032 : }
2033 367254 : return true;
2034 : }
2035 :
2036 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
2037 : /*
2038 : get the body-frame moving baseline antenna offset used to calculate the yaw
2039 : returned by gps_yaw_deg, zero when that yaw is not derived from a moving
2040 : baseline. The yaw is calculated assuming the offset is horizontal, so
2041 : consumers with an attitude estimate can use this offset to correct the yaw
2042 : for vehicle roll and pitch
2043 : */
2044 123468855 : const Vector3f &AP_GPS::get_mb_yaw_offset(uint8_t instance) const
2045 : {
2046 : // resolve the same instance whose yaw gps_yaw_deg reports, so the offset
2047 : // always describes the yaw it accompanies
2048 123468855 : const uint8_t yaw_instance = yaw_source_instance(instance);
2049 123468855 : return state[yaw_instance].mb_yaw_offset;
2050 : }
2051 : #endif // AP_GPS_MB_YAW_OFFSET_ENABLED
2052 :
2053 : /*
2054 : * Old parameter metadata. Until we have versioned parameters, keeping
2055 : * old parameters around for a while can help with an adjustment
2056 : * period.
2057 : */
2058 :
2059 : // @Param: _TYPE
2060 : // @DisplayName: 1st GPS type
2061 : // @Description: GPS type of 1st GPS.Renamed in 4.6 and later to GPS1_TYPE
2062 : // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna
2063 : // @RebootRequired: True
2064 : // @User: Advanced
2065 : // @Legacy: only included here so GCSs running stable can get the description. Omitted in the Wiki.
2066 :
2067 : // @Param: _TYPE2
2068 : // @CopyFieldsFrom: GPS_TYPE
2069 : // @DisplayName: 2nd GPS type.Renamed in 4.6 to GPS2_TYPE
2070 : // @Description: GPS type of 2nd GPS
2071 : // @Legacy: 4.5 param
2072 :
2073 : // @Param: _GNSS_MODE
2074 : // @DisplayName: GNSS system configuration
2075 : // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured).Renamed in 4.6 and later to GPS1_GNSS_MODE.
2076 : // @Legacy: 4.5 param
2077 : // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
2078 : // @User: Advanced
2079 :
2080 : // @Param: _GNSS_MODE2
2081 : // @DisplayName: GNSS system configuration.
2082 : // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured). Renamed in 4.6 and later to GPS2_GNSS_MODE
2083 : // @Legacy: 4.5 param
2084 : // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
2085 : // @User: Advanced
2086 :
2087 : // @Param: _RATE_MS
2088 : // @DisplayName: GPS update rate in milliseconds
2089 : // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.Renamed in 4.6 and later to GPS1_RATE_MS
2090 : // @Legacy: 4.5 param
2091 : // @Units: ms
2092 : // @Values: 100:10Hz,125:8Hz,200:5Hz
2093 : // @Range: 50 200
2094 : // @User: Advanced
2095 :
2096 : // @Param: _RATE_MS2
2097 : // @DisplayName: GPS 2 update rate in milliseconds
2098 : // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.Renamed in 4.6 and later to GPS2_RATE_MS
2099 : // @Legacy: 4.5 param
2100 : // @Units: ms
2101 : // @Values: 100:10Hz,125:8Hz,200:5Hz
2102 : // @Range: 50 200
2103 : // @User: Advanced
2104 :
2105 : // @Param: _POS1_X
2106 : // @DisplayName: Antenna X position offset
2107 : // @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_X.
2108 : // @Legacy: 4.5 param
2109 : // @Units: m
2110 : // @Range: -5 5
2111 : // @Increment: 0.01
2112 : // @User: Advanced
2113 :
2114 : // @Param: _POS1_Y
2115 : // @DisplayName: Antenna Y position offset
2116 : // @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_Y.
2117 : // @Legacy: 4.5 param
2118 : // @Units: m
2119 : // @Range: -5 5
2120 : // @Increment: 0.01
2121 : // @User: Advanced
2122 :
2123 : // @Param: _POS1_Z
2124 : // @DisplayName: Antenna Z position offset
2125 : // @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_Z.
2126 : // @Legacy: 4.5 param
2127 : // @Units: m
2128 : // @Range: -5 5
2129 : // @Increment: 0.01
2130 : // @User: Advanced
2131 :
2132 : // @Param: _POS2_X
2133 : // @DisplayName: Antenna X position offset
2134 : // @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_X.
2135 : // @Legacy: 4.5 param
2136 : // @Units: m
2137 : // @Range: -5 5
2138 : // @Increment: 0.01
2139 : // @User: Advanced
2140 :
2141 : // @Param: _POS2_Y
2142 : // @DisplayName: Antenna Y position offset
2143 : // @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_Y.
2144 : // @Legacy: 4.5 param
2145 : // @Units: m
2146 : // @Range: -5 5
2147 : // @Increment: 0.01
2148 : // @User: Advanced
2149 :
2150 : // @Param: _POS2_Z
2151 : // @DisplayName: Antenna Z position offset
2152 : // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_Z.
2153 : // @Legacy: 4.5 param
2154 : // @Units: m
2155 : // @Range: -5 5
2156 : // @Increment: 0.01
2157 : // @User: Advanced
2158 :
2159 : // @Param: _DELAY_MS
2160 : // @DisplayName: GPS delay in milliseconds
2161 : // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.Renamed in 4.6 and later to GPS1_DELAY_MS.
2162 : // @Legacy: 4.5 param
2163 : // @Units: ms
2164 : // @Range: 0 250
2165 : // @User: Advanced
2166 : // @RebootRequired: True
2167 :
2168 : // @Param: _DELAY_MS2
2169 : // @DisplayName: GPS 2 delay in milliseconds
2170 : // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.Renamed in 4.6 and later to GPS2_DELAY_MS.
2171 : // @Legacy: 4.5 param
2172 : // @Units: ms
2173 : // @Range: 0 250
2174 : // @User: Advanced
2175 : // @RebootRequired: True
2176 :
2177 : // @Param: _COM_PORT
2178 : // @DisplayName: GPS physical COM port
2179 : // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS,Renamed in 4.6 and later to GPS1_COM_PORT.
2180 : // @Legacy: 4.5 param
2181 : // @Range: 0 10
2182 : // @Increment: 1
2183 : // @User: Advanced
2184 : // @Values: 0:COM1(RS232) on GSOF, 1:COM2(TTL) on GSOF
2185 : // @RebootRequired: True
2186 :
2187 : // @Param: _COM_PORT2
2188 : // @DisplayName: GPS physical COM port
2189 : // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS.Renamed in 4.6 and later to GPS1_COM_PORT.
2190 : // @Legacy: 4.5 param
2191 : // @Range: 0 10
2192 : // @Increment: 1
2193 : // @User: Advanced
2194 : // @RebootRequired: True
2195 :
2196 : // @Group: _MB1_
2197 : // @Path: MovingBase.cpp
2198 : // @Legacy: 4.5 param
2199 :
2200 : // @Group: _MB2_
2201 : // @Path: MovingBase.cpp
2202 : // @Legacy: 4.5 param
2203 :
2204 : // @Param: _CAN_NODEID1
2205 : // @DisplayName: GPS Node ID 1
2206 : // @Description: GPS Node id for first-discovered GPS.Renamed in 4.6 and later to GPS1_CAN_NODEID.
2207 : // @Legacy: 4.5 param
2208 : // @ReadOnly: True
2209 : // @User: Advanced
2210 :
2211 : // @Param: _CAN_NODEID2
2212 : // @DisplayName: GPS Node ID 2
2213 : // @Description: GPS Node id for second-discovered GPS.Renamed in 4.6 and later to GPS2_CAN_NODEID.
2214 : // @Legacy: 4.5 param
2215 : // @ReadOnly: True
2216 : // @User: Advanced
2217 :
2218 : /*
2219 : * end old parameter metadata
2220 : */
2221 :
2222 : namespace AP {
2223 :
2224 478764882 : AP_GPS &gps()
2225 : {
2226 478764882 : return *AP_GPS::get_singleton();
2227 : }
2228 :
2229 : };
2230 :
2231 : #endif // AP_GPS_ENABLED
|
