diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 4e2ccc5c840b9..426b1666c2ca3 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -613,8 +613,10 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) */ void AP_GPS::send_blob_start(uint8_t instance) { + const auto type = _type[instance]; + #if AP_GPS_UBLOX_ENABLED - if (_type[instance] == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) { + if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) { static const char blob[] = UBLOX_SET_BINARY_115200; send_blob_start(instance, blob, sizeof(blob)); return; @@ -622,8 +624,8 @@ void AP_GPS::send_blob_start(uint8_t instance) #endif // AP_GPS_UBLOX_ENABLED #if GPS_MOVING_BASELINE && AP_GPS_UBLOX_ENABLED - if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || - _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && + if ((type == GPS_TYPE_UBLOX_RTK_BASE || + type == GPS_TYPE_UBLOX_RTK_ROVER) && !option_set(DriverOptions::UBX_MBUseUart2)) { // we use 460800 when doing moving baseline as we need // more bandwidth. We don't do this if using UART2, as @@ -638,7 +640,7 @@ void AP_GPS::send_blob_start(uint8_t instance) // the following devices don't have init blobs: const char *blob = nullptr; uint32_t blob_size = 0; - switch (_type[instance]) { + switch (type) { #if AP_GPS_SBF_ENABLED case GPS_TYPE_SBF: case GPS_TYPE_SBF_DUAL_ANTENNA: @@ -723,7 +725,9 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) { struct detect_state *dstate = &detect_state[instance]; - switch (_type[instance]) { + const auto type = _type[instance]; + + switch (type) { // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there case GPS_TYPE_MAV: @@ -784,10 +788,10 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) dstate->probe_baud = _baudrates[dstate->current_baud]; } uint16_t rx_size=0, tx_size=0; - if (_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) { + if (type == GPS_TYPE_UBLOX_RTK_ROVER) { tx_size = 2048; } - if (_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) { + if (type == GPS_TYPE_UBLOX_RTK_BASE) { rx_size = 2048; } _port[instance]->begin(dstate->probe_baud, rx_size, tx_size); @@ -803,7 +807,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) send_blob_update(instance); } - switch (_type[instance]) { + switch (type) { #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: @@ -836,8 +840,8 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) (void)data; // if all backends are compiled out then "data" is unused #if AP_GPS_UBLOX_ENABLED - if ((_type[instance] == GPS_TYPE_AUTO || - _type[instance] == GPS_TYPE_UBLOX) && + if ((type == GPS_TYPE_AUTO || + type == GPS_TYPE_UBLOX) && ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) || (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) || _baudrates[dstate->current_baud] == 230400) && @@ -846,12 +850,12 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) } const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800; - if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || - _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && + if ((type == GPS_TYPE_UBLOX_RTK_BASE || + type == GPS_TYPE_UBLOX_RTK_ROVER) && _baudrates[dstate->current_baud] == ublox_mb_required_baud && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { GPS_Role role; - if (_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) { + if (type == GPS_TYPE_UBLOX_RTK_BASE) { role = GPS_ROLE_MB_BASE; } else { role = GPS_ROLE_MB_ROVER; @@ -860,37 +864,37 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) } #endif // AP_GPS_UBLOX_ENABLED #if AP_GPS_SBP2_ENABLED - if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) && AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { return new AP_GPS_SBP2(*this, state[instance], _port[instance]); } #endif //AP_GPS_SBP2_ENABLED #if AP_GPS_SBP_ENABLED - if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) && AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { return new AP_GPS_SBP(*this, state[instance], _port[instance]); } #endif //AP_GPS_SBP_ENABLED #if AP_GPS_SIRF_ENABLED - if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && + if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SIRF) && AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { return new AP_GPS_SIRF(*this, state[instance], _port[instance]); } #endif #if AP_GPS_ERB_ENABLED - if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && + if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_ERB) && AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { return new AP_GPS_ERB(*this, state[instance], _port[instance]); } #endif // AP_GPS_ERB_ENABLED #if AP_GPS_NMEA_ENABLED - if ((_type[instance] == GPS_TYPE_NMEA || - _type[instance] == GPS_TYPE_HEMI || + if ((type == GPS_TYPE_NMEA || + type == GPS_TYPE_HEMI || #if AP_GPS_NMEA_UNICORE_ENABLED - _type[instance] == GPS_TYPE_UNICORE_NMEA || - _type[instance] == GPS_TYPE_UNICORE_MOVINGBASE_NMEA || + type == GPS_TYPE_UNICORE_NMEA || + type == GPS_TYPE_UNICORE_MOVINGBASE_NMEA || #endif - _type[instance] == GPS_TYPE_ALLYSTAR) && + type == GPS_TYPE_ALLYSTAR) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { return new AP_GPS_NMEA(*this, state[instance], _port[instance]); } @@ -933,11 +937,12 @@ bool AP_GPS::should_log() const */ void AP_GPS::update_instance(uint8_t instance) { - if (_type[instance] == GPS_TYPE_HIL) { + const auto type = _type[instance]; + if (type == GPS_TYPE_HIL) { // in HIL, leave info alone return; } - if (_type[instance] == GPS_TYPE_NONE) { + if (type == GPS_TYPE_NONE) { // not enabled state[instance].status = NO_GPS; state[instance].hdop = GPS_UNKNOWN_DOP; @@ -977,10 +982,10 @@ void AP_GPS::update_instance(uint8_t instance) timing[instance].last_message_time_ms = tnow; timing[instance].delta_time_ms = GPS_TIMEOUT_MS; // do not try to detect again if type is MAV or UAVCAN - if (_type[instance] == GPS_TYPE_MAV || - _type[instance] == GPS_TYPE_UAVCAN || - _type[instance] == GPS_TYPE_UAVCAN_RTK_BASE || - _type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER) { + if (type == GPS_TYPE_MAV || + type == GPS_TYPE_UAVCAN || + type == GPS_TYPE_UAVCAN_RTK_BASE || + type == GPS_TYPE_UAVCAN_RTK_ROVER) { state[instance].status = NO_FIX; } else { // free the driver before we run the next detection, so we @@ -1020,7 +1025,7 @@ void AP_GPS::update_instance(uint8_t instance) } #if GPS_MAX_RECEIVERS > 1 - if (drivers[instance] && _type[instance] == GPS_TYPE_UBLOX_RTK_BASE) { + if (drivers[instance] && type == GPS_TYPE_UBLOX_RTK_BASE) { // see if a moving baseline base has some RTCMv3 data // which we need to pass along to the rover const uint8_t *rtcm_data; @@ -1177,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 @@ -1232,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; @@ -1391,7 +1393,7 @@ bool AP_GPS::get_first_external_instance(uint8_t& instance) const void AP_GPS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt, const uint8_t instance) { - if (_type[instance] == GPS_TYPE_EXTERNAL_AHRS && drivers[instance] != nullptr) { + if (get_type(instance) == GPS_TYPE_EXTERNAL_AHRS && drivers[instance] != nullptr) { drivers[instance]->handle_external(pkt); } } @@ -1421,7 +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; iget_lag(lag_sec); @@ -2248,7 +2251,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const fact that the rate of yaw data is not critical */ const uint8_t delay_threshold = 2; - const float delay_avg_max = ((_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER))?333:215; + const float delay_avg_max = is_rtk_rover(instance) ? 333 : 215; const GPS_timing &t = timing[instance]; bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max && @@ -2281,16 +2284,16 @@ bool AP_GPS::prepare_for_arming(void) { bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { #if AP_GPS_DRONECAN_ENABLED - if (_type[i] == GPS_TYPE_UAVCAN || - _type[i] == GPS_TYPE_UAVCAN_RTK_BASE || - _type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) { + const auto type = _type[i]; + if (type == GPS_TYPE_UAVCAN || + type == GPS_TYPE_UAVCAN_RTK_BASE || + type == GPS_TYPE_UAVCAN_RTK_ROVER) { if (!AP_GPS_DroneCAN::backends_healthy(failure_msg, failure_msg_len)) { return false; } } #endif - if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER || - _type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) { + if (is_rtk_rover(i)) { if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) { hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1)); return false; @@ -2402,15 +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 - if (instance < GPS_MAX_RECEIVERS && - ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_BASE)) && - ((_type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (_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 c87eb3f6b2759..3efa390427200 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -138,6 +138,10 @@ class AP_GPS #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 {