diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 43d6f71e945ab..d6a541a06d058 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1808,18 +1808,17 @@ bool AP_GPS::prepare_for_arming(void) { return all_passed; } -bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { +bool AP_GPS::pre_arm_checks(char failure_msg[], uint16_t failure_msg_len) +{ + // the DroneCAN class has additional checks for DroneCAN-specific + // parameters: #if AP_GPS_DRONECAN_ENABLED - const auto type = params[i].type; - 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; - } - } + if (!AP_GPS_DroneCAN::inter_instance_pre_arm_checks(failure_msg, failure_msg_len)) { + return false; + } #endif + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { 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)); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index e21bf66524631..25ea5a2f9bae2 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -551,9 +551,10 @@ class AP_GPS // returns true if all GPS instances have passed all final arming checks/state changes bool prepare_for_arming(void); - // returns true if all GPS backend drivers haven't seen any failure - // this is for backends to be able to spout pre arm error messages - bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); + // returns true if all GPS backend drivers are OK with the concept + // of the vehicle arming. this is for backends to be able to + // spout pre arm error messages + bool pre_arm_checks(char failure_msg[], uint16_t failure_msg_len); // returns false if any GPS drivers are not performing their logging appropriately bool logging_failed(void) const; diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index a2130fdbdc299..a011e69c695af 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -225,8 +225,24 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) return backend; } -bool AP_GPS_DroneCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len) +bool AP_GPS_DroneCAN::inter_instance_pre_arm_checks(char failure_msg[], uint16_t failure_msg_len) { + // only do these checks if there is at least one DroneCAN GPS: + bool found_uavan_type = false; + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const auto type = AP::gps().params[i].type; + if (type != AP_GPS::GPS_TYPE_UAVCAN && + type != AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE && + type != AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER) { + continue; + } + found_uavan_type = true; + } + if (!found_uavan_type) { + return true; + } + + // lint parameters and detected node IDs: for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { const auto ¶ms_i = AP::gps().params[i]; bool overriden_node_found = false; diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 200d8ae11c3ff..f422c983fe34c 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -56,7 +56,7 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend { static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg); #endif - static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); + static bool inter_instance_pre_arm_checks(char failure_msg[], uint16_t failure_msg_len); void inject_data(const uint8_t *data, uint16_t len) override; bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };