diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c055e386b4..70d29a4ba4 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -101,8 +101,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #if GPS_MAX_RECEIVERS > 1 // @Param: AUTO_SWITCH // @DisplayName: Automatic Switchover Setting - // @Description: Automatic switchover to GPS reporting best lock - // @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond + // @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 + // @Values: 0:Use primary, 1:UseBest, 2:Blend // @User: Advanced AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST), #endif @@ -336,6 +336,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #endif // GPS_MOVING_BASELINE +#if GPS_MAX_RECEIVERS > 1 + // @Param: PRIMARY + // @DisplayName: Primary GPS + // @Description: This GPS will be used when GPS_AUTO_SWITCH is 0 and used preferentially with GPS_AUTO_SWITCH = 4. + // @Increment: 1 + // @Values: 0:FirstGPS, 1:SecondGPS + // @User: Advanced + AP_GROUPINFO("PRIMARY", 27, AP_GPS, _primary, 0), +#endif + AP_GROUPEND }; @@ -372,7 +382,11 @@ bool AP_GPS::needs_uart(GPS_Type type) const /// Startup initialisation. void AP_GPS::init(const AP_SerialManager& serial_manager) { - primary_instance = 0; + // Set new primary param based on old auto_switch use second option + if ((_auto_switch.get() == 3) && !_primary.configured()) { + _primary.set_and_save(1); + _auto_switch.set_and_save(0); + } // search for serial ports with gps protocol uint8_t uart_idx = 0; @@ -903,15 +917,19 @@ void AP_GPS::update_primary(void) return; } - if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::NONE) { - // AUTO_SWITCH is 0 so no switching of GPSs, always use first instance - primary_instance = 0; - return; + // check the primary param is set to possible GPS + int8_t primary_param = _primary.get(); + if ((primary_param < 0) || (primary_param>=GPS_MAX_RECEIVERS)) { + primary_param = 0; + } + // if primary is not enabled try first instance + if (get_type(primary_param) == GPS_TYPE_NONE) { + primary_param = 0; } - if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_SECOND) { - // always select the second GPS instance - primary_instance = 1; + if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::NONE) { + // No switching of GPSs, always use the primary instance + primary_instance = primary_param; return; } @@ -1782,6 +1800,10 @@ bool AP_GPS::is_healthy(uint8_t instance) const return false; } + if (get_type(_primary.get()) == GPS_TYPE_NONE) { + return false; + } + /* allow two lost frames before declaring the GPS unhealthy, but require the average frame rate to be close to 5Hz. We allow for diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 0bb37527e2..bbb2e5fc9a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -659,7 +659,7 @@ private: NONE = 0, USE_BEST = 1, BLEND = 2, - USE_SECOND = 3, + //USE_SECOND = 3, deprecated for new primary param }; // used for flight testing with GPS loss