From 747c708dbafd51147accd54ec78c75986edf1643 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Tue, 11 Jul 2023 17:19:53 +0530 Subject: [PATCH] AP_RangeFinder: Have special handling for NRA24 pre-arm checks --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 19 +++++++++++++++++-- .../AP_RangeFinder_NRA24_CAN.cpp | 10 ++++++---- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index a793462846..7376069717 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -794,6 +794,7 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le // backend-specific checks. This might end up drivers[i]->arming_checks(...). switch (drivers[i]->allocated_type()) { +#if AP_RANGEFINDER_PWM_ENABLED || AP_RANGEFINDER_ANALOG_ENABLED case Type::ANALOG: case Type::PX4_PWM: case Type::PWM: { @@ -819,7 +820,21 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le } break; } -default: +#endif + +#if AP_RANGEFINDER_NRA24_CAN_ENABLED + case Type::NRA24_CAN: { + if (drivers[i]->status() == Status::NoData) { + // This sensor stops sending data if there is no relative motion. This will mostly happen during takeoff, before arming + // To avoid pre-arm failure, return true even though there is no data. + // This sensor also sends a "heartbeat" so we can differentiate between "NoData" and "NotConnected" + return true; + } + break; + } +#endif + + default: break; } @@ -832,7 +847,7 @@ default: return false; case Status::OutOfRangeLow: case Status::OutOfRangeHigh: - case Status::Good: + case Status::Good: break; } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp index f457dc2ae0..61715cc58c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp @@ -36,11 +36,13 @@ void AP_RangeFinder_NRA24_CAN::update(void) state.last_reading_ms = AP_HAL::millis(); update_status(); } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) { - if (AP_HAL::millis() - last_heartbeat_ms < read_timeout_ms()) { - // don't have distance data but sensor is connected. This is a known issue when sensor is in static condition (example Copter waiting to take off). Set status to out of range to avoid pre arm error - set_status(RangeFinder::Status::OutOfRangeLow); - } else { + if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) { + // no heartbeat, must be disconnected set_status(RangeFinder::Status::NotConnected); + } else { + // Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar. + // This case has special pre-arm check handling + set_status(RangeFinder::Status::NoData); } } }