Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: add use primary if 3D fix GPS_AUTO_SWITCH option

Browse Source
This commit is contained in:
Iampete1 2020-10-29 22:56:09 +00:00 committed by Andrew Tridgell
parent 347723dbaf
commit 60951a7891
2 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_GPS/AP_GPS.cpp
View File

@ -101,8 +101,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
#if GPS_MAX_RECEIVERS > 1 #if GPS_MAX_RECEIVERS > 1
// @Param: AUTO_SWITCH // @Param: AUTO_SWITCH
// @DisplayName: Automatic Switchover Setting // @DisplayName: Automatic Switchover Setting
// @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 // @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 over 'UseBest' behaviour if 3D fix is lost on primary
// @Values: 0:Use primary, 1:UseBest, 2:Blend // @Values: 0:Use primary, 1:UseBest, 2:Blend, 4:Use primary if 3D fix or better
// @User: Advanced // @User: Advanced
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST), AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST),
#endif #endif
@ -988,6 +988,17 @@ void AP_GPS::update_primary(void)
return; return;
} }
// Use primary if 3D fix or better
if (((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_PRIMARY_IF_3D_FIX) && (state[primary_param].status >= GPS_OK_FIX_3D)) {
// Primary GPS has a least a 3D fix, switch to it if necessary
if (primary_instance != primary_param) {
primary_instance = primary_param;
_last_instance_swap_ms = now;
}
return;
}
// handle switch between real GPSs // handle switch between real GPSs
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (i == primary_instance) { if (i == primary_instance) {

2
libraries/AP_GPS/AP_GPS.h
View File

@ -522,6 +522,7 @@ protected:
AP_Int8 _blend_mask; AP_Int8 _blend_mask;
AP_Float _blend_tc; AP_Float _blend_tc;
AP_Int16 _driver_options; AP_Int16 _driver_options;
AP_Int8 _primary;
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS]; MovingBase mb_params[GPS_MAX_RECEIVERS];
@ -660,6 +661,7 @@ private:
USE_BEST = 1, USE_BEST = 1,
BLEND = 2, BLEND = 2,
//USE_SECOND = 3, deprecated for new primary param //USE_SECOND = 3, deprecated for new primary param
USE_PRIMARY_IF_3D_FIX = 4,
}; };
// used for flight testing with GPS loss // used for flight testing with GPS loss