Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: tidy pre_arm_checks

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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));

7
libraries/AP_GPS/AP_GPS.h
View File

@ -551,9 +551,10 @@ public:
// 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;

18
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -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 &params_i = AP::gps().params[i];
bool overriden_node_found = false;

2
libraries/AP_GPS/AP_GPS_DroneCAN.h
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_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; };