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

AP_SurfaceDistance: Add Rangefinder configured check and out of range low timer

Browse Source
This commit is contained in:
MattKear 2024-10-16 18:30:03 +01:00
parent f90f2810ae
commit d5337c3c93
2 changed files with 60 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp
View File

@ -64,6 +64,20 @@ void AP_SurfaceDistance::update()
// update health
alt_healthy = (rangefinder->status_orient(rotation) == RangeFinder::Status::Good) &&
(rangefinder->range_valid_count_orient(rotation) >= RANGEFINDER_HEALTH_MIN);
// Keep track of whether we have ever had a healthy reading
has_been_healthy = has_been_healthy || alt_healthy;
// reset the out of range low timer if reading is reporting any other status
if (rangefinder->status_orient(rotation) != RangeFinder::Status::OutOfRangeLow) {
out_of_range_low_ms = 0;
}
// if we have just changed state to out of range low then keep track of the time this happened
if (out_of_range_low_ms == 0 && rangefinder->status_orient(rotation) == RangeFinder::Status::OutOfRangeLow) {
out_of_range_low_ms = now;
}
if (!alt_healthy) {
status |= (uint8_t)Surface_Distance_Status::Unhealthy;
}
@ -134,11 +148,37 @@ void AP_SurfaceDistance::update()
difference between the inertial height at that time and the current
inertial height to give us interpolation of height from rangefinder
*/
bool AP_SurfaceDistance::get_rangefinder_height_interpolated_cm(int32_t& ret) const
bool AP_SurfaceDistance::get_rangefinder_height_interpolated_cm(int32_t& ret, const uint32_t oor_low_timeout_ms)
{
if (!enabled_and_healthy()) {
WITH_SEMAPHORE(sem);
// surface distance state not enabled
if (!enabled) {
return false;
}
// rangefinder has never been healthy so we cannot trust an interpolated result
if (!has_been_healthy) {
return false;
}
// rangefinder is unhealthy and we have not been told to temporarily accept out of range low values so we cannot use this measurement
if (!alt_healthy && oor_low_timeout_ms == 0) {
return false;
}
// we don't have an out of range low measurement and the rangefinder state is reporting unhealthy
if (!alt_healthy && out_of_range_low_ms == 0) {
return false;
}
// we are unhealthy because of an out of range low ready and the timer has run out on how long we are allowed to use a potentially out of range low reading
// TODO: handle timer wrap case?
if (!alt_healthy && (out_of_range_low_ms > 0) && (AP_HAL::millis() - out_of_range_low_ms > oor_low_timeout_ms)) {
return false;
}
// if we have got this far the rangefinder is enabled and is either healthy or we are within the permited
// grace time to be able to still use the rangefinder even though it is reporting out of range low
ret = alt_cm_filt.get();
ret += inertial_nav.get_position_z_up_cm() - inertial_alt_cm;
return true;
@ -185,3 +225,13 @@ bool AP_SurfaceDistance::enabled_and_healthy(void) const
{
return enabled && alt_healthy;
}
bool AP_SurfaceDistance::rangefinder_configured(void) const
{
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder == nullptr) {
return false;
}
return rangefinder->status_orient(rotation) != RangeFinder::Status::NotConnected &&
rangefinder->status_orient(rotation) != RangeFinder::Status::NoData;
}

9
libraries/AP_SurfaceDistance/AP_SurfaceDistance.h
View File

@ -21,8 +21,12 @@ public:
// helper to check that rangefinder was last reported as enabled and healthy
bool enabled_and_healthy(void) const;
// Check that there is a rangefinder in the correct orientation configured, used for pre-arm checks
// when the rangefinder may not be reporting healthy (e.g. on ground reporting out of range low)
bool rangefinder_configured(void) const;
// get inertially interpolated rangefinder height
bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
bool get_rangefinder_height_interpolated_cm(int32_t& ret, const uint32_t oor_low_timeout_ms = 0);
bool enabled; // not to be confused with rangefinder enabled, this state is to be set by the vehicle.
bool alt_healthy; // true if we can trust the altitude from the rangefinder
@ -42,6 +46,9 @@ private:
// multi-thread access support
HAL_Semaphore sem;
bool rangefinder_is_config;
bool has_been_healthy;
uint32_t out_of_range_low_ms; // keep track of the rangefinder state. When using the inertially interpolated rangefinder reading sometimes it is acceptable to use the rangefinder when it is reporting low
const uint8_t instance;
uint8_t status;
uint32_t last_healthy_ms;