AP_GPS: tidy pre_arm_checks

This commit is contained in:
Peter Barker 2024-05-22 14:29:39 +10:00 committed by Andrew Tridgell
parent 3e57edf37c
commit ebfbcacfe3
4 changed files with 31 additions and 15 deletions

View File

@ -1808,18 +1808,17 @@ bool AP_GPS::prepare_for_arming(void) {
return all_passed; return all_passed;
} }
bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { bool AP_GPS::pre_arm_checks(char failure_msg[], uint16_t failure_msg_len)
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { {
// the DroneCAN class has additional checks for DroneCAN-specific
// parameters:
#if AP_GPS_DRONECAN_ENABLED #if AP_GPS_DRONECAN_ENABLED
const auto type = params[i].type; if (!AP_GPS_DroneCAN::inter_instance_pre_arm_checks(failure_msg, failure_msg_len)) {
if (type == GPS_TYPE_UAVCAN || return false;
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 #endif
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (is_rtk_rover(i)) { if (is_rtk_rover(i)) {
if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) { 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)); hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1));

View File

@ -551,9 +551,10 @@ public:
// returns true if all GPS instances have passed all final arming checks/state changes // returns true if all GPS instances have passed all final arming checks/state changes
bool prepare_for_arming(void); bool prepare_for_arming(void);
// returns true if all GPS backend drivers haven't seen any failure // returns true if all GPS backend drivers are OK with the concept
// this is for backends to be able to spout pre arm error messages // of the vehicle arming. this is for backends to be able to
bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); // 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 // returns false if any GPS drivers are not performing their logging appropriately
bool logging_failed(void) const; bool logging_failed(void) const;

View File

@ -225,8 +225,24 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
return backend; 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++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
const auto &params_i = AP::gps().params[i]; const auto &params_i = AP::gps().params[i];
bool overriden_node_found = false; bool overriden_node_found = false;

View File

@ -56,7 +56,7 @@ public:
static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); 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); static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg);
#endif #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; 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; }; bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };