diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index fb51c2df81..426b1666c2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1182,10 +1182,7 @@ void AP_GPS::update_primary(void) significant lagged and gives no more information on position or velocity */ - const bool using_moving_base = (_type[0] == GPS_TYPE_UAVCAN_RTK_BASE || - _type[0] == GPS_TYPE_UBLOX_RTK_BASE || - _type[1] == GPS_TYPE_UAVCAN_RTK_BASE || - _type[1] == GPS_TYPE_UBLOX_RTK_BASE); + const bool using_moving_base = (is_rtk_base(_type[0]) || is_rtk_base(_type[1])); if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::BLEND && !using_moving_base) { _output_is_blended = calc_blend_weights(); // adjust blend health counter @@ -1237,8 +1234,8 @@ void AP_GPS::update_primary(void) // rover as it typically is in fix type 6 (RTK) whereas base is // usually fix type 3 for (uint8_t i=0; i= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) { if (primary_instance != i) { _last_instance_swap_ms = now; @@ -1426,8 +1423,7 @@ void AP_GPS::inject_data(const uint8_t *data, uint16_t len) //Support broadcasting to all GPSes. if (_inject_to == GPS_RTK_INJECT_TO_ALL) { for (uint8_t i=0; i 15000) { hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1)); return false; @@ -2411,16 +2405,37 @@ void AP_GPS::Write_GPS(uint8_t i) } #endif +bool AP_GPS::is_rtk_base(uint8_t instance) const +{ + switch (get_type(instance)) { + case GPS_TYPE_UBLOX_RTK_BASE: + case GPS_TYPE_UAVCAN_RTK_BASE: + return true; + default: + break; + } + return false; +} + +bool AP_GPS::is_rtk_rover(uint8_t instance) const +{ + switch (get_type(instance)) { + case GPS_TYPE_UBLOX_RTK_ROVER: + case GPS_TYPE_UAVCAN_RTK_ROVER: + return true; + default: + break; + } + return false; +} + /* get GPS based yaw */ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const { #if GPS_MAX_RECEIVERS > 1 - const auto type = get_type(instance); - if ( - ((type == GPS_TYPE_UBLOX_RTK_BASE) || (type == GPS_TYPE_UAVCAN_RTK_BASE)) && - ((get_type(instance^1) == GPS_TYPE_UBLOX_RTK_ROVER) || (get_type(instance^1) == GPS_TYPE_UAVCAN_RTK_ROVER))) { + if (is_rtk_base(instance) && is_rtk_rover(instance^1)) { // return the yaw from the rover instance ^= 1; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index c87eb3f6b2..3efa390427 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -138,6 +138,10 @@ public: #endif }; + // convenience methods for working out what general type an instance is: + bool is_rtk_base(uint8_t instance) const; + bool is_rtk_rover(uint8_t instance) const; + /// GPS status codes. These are kept aligned with MAVLink by /// static_assert in AP_GPS.cpp enum GPS_Status {