From bb431b7e0ab83dbc392bbb922e5ff47860f07c69 Mon Sep 17 00:00:00 2001 From: MattKear Date: Wed, 1 May 2024 13:50:30 +0100 Subject: [PATCH 1/2] AP_SurfaceDistance: Start library for tracking the floor/roof distance --- .../AP_SurfaceDistance/AP_SurfaceDistance.cpp | 187 ++++++++++++++++++ .../AP_SurfaceDistance/AP_SurfaceDistance.h | 47 +++++ 2 files changed, 234 insertions(+) create mode 100644 libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp create mode 100644 libraries/AP_SurfaceDistance/AP_SurfaceDistance.h diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp new file mode 100644 index 0000000000000..af8ec0b42f091 --- /dev/null +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.cpp @@ -0,0 +1,187 @@ +#include "AP_SurfaceDistance.h" + +#include + +#ifndef RANGEFINDER_TIMEOUT_MS + # define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second +#endif + +#if AP_RANGEFINDER_ENABLED + +#include +#include + +#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF + # define RANGEFINDER_TILT_CORRECTION 1 +#endif + +#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES + # define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading +#endif + +#ifndef RANGEFINDER_GLITCH_ALT_CM + # define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch +#endif + +#ifndef RANGEFINDER_HEALTH_MIN + # define RANGEFINDER_HEALTH_MIN 3 // number of good reads that indicates a healthy rangefinder +#endif + +void AP_SurfaceDistance::update() +{ + WITH_SEMAPHORE(sem); + + const RangeFinder *rangefinder = RangeFinder::get_singleton(); + if (rangefinder == nullptr) { + alt_healthy = false; + return; + } + +#if RANGEFINDER_TILT_CORRECTION == 1 + const float tilt_correction = MAX(0.707f, AP::ahrs().get_rotation_body_to_ned().c.z); +#else + const float tilt_correction = 1.0f; +#endif + + const uint32_t now = AP_HAL::millis(); + + // assemble bitmask assistance, definition is used to generate log documentation + enum class Surface_Distance_Status : uint8_t { + Enabled = 1U<<0, // true if rangefinder has been set to enable by vehicle + Unhealthy = 1U<<1, // true if rangefinder is considered unhealthy + Stale_Data = 1U<<2, // true if the last healthy rangefinder reading is no longer valid + Glitch_Detected = 1U<<3, // true if a measurement glitch detected + }; + + // reset status and add to the bitmask as we progress through the update + status = 0; + + // update enabled status + if (enabled) { + status |= (uint8_t)Surface_Distance_Status::Enabled; + } + + // update health + alt_healthy = (rangefinder->status_orient(rotation) == RangeFinder::Status::Good) && + (rangefinder->range_valid_count_orient(rotation) >= RANGEFINDER_HEALTH_MIN); + if (!alt_healthy) { + status |= (uint8_t)Surface_Distance_Status::Unhealthy; + } + + // tilt corrected but unfiltered, not glitch protected alt + alt_cm = tilt_correction * rangefinder->distance_cm_orient(rotation); + + // remember inertial alt to allow us to interpolate rangefinder + inertial_alt_cm = inertial_nav.get_position_z_up_cm(); + + // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading + // are considered a glitch and glitch_count becomes non-zero + // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row. + // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset + const int32_t glitch_cm = alt_cm - alt_cm_glitch_protected; + bool reset_terrain_offset = false; + if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { + glitch_count = MAX(glitch_count+1, 1); + status |= (uint8_t)Surface_Distance_Status::Glitch_Detected; + } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { + glitch_count = MIN(glitch_count-1, -1); + status |= (uint8_t)Surface_Distance_Status::Glitch_Detected; + } else { + glitch_count = 0; + alt_cm_glitch_protected = alt_cm; + } + if (abs(glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { + // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes + glitch_count = 0; + alt_cm_glitch_protected = alt_cm; + glitch_cleared_ms = now; + reset_terrain_offset = true; + } + + // filter rangefinder altitude + const bool timed_out = now - last_healthy_ms > RANGEFINDER_TIMEOUT_MS; + if (alt_healthy) { + if (timed_out) { + // reset filter if we haven't used it within the last second + alt_cm_filt.reset(alt_cm); + reset_terrain_offset = true; + status |= (uint8_t)Surface_Distance_Status::Stale_Data; + } else { + // TODO: When we apply this library in plane we will need to be able to set the filter freq + alt_cm_filt.apply(alt_cm, 0.05); + } + last_healthy_ms = now; + } + + // handle reset of terrain offset + if (reset_terrain_offset) { + if (rotation == ROTATION_PITCH_90) { + // upward facing + terrain_offset_cm = inertial_alt_cm + alt_cm; + } else { + // assume downward facing + terrain_offset_cm = inertial_alt_cm - alt_cm; + } + } +#if HAL_LOGGING_ENABLED + Log_Write(); +#endif +} + +/* + get inertially interpolated rangefinder height. Inertial height is + recorded whenever we update the rangefinder height, then we use the + 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 +{ + if (!enabled_and_healthy()) { + return false; + } + ret = alt_cm_filt.get(); + ret += inertial_nav.get_position_z_up_cm() - inertial_alt_cm; + return true; +} + +#if HAL_LOGGING_ENABLED +void AP_SurfaceDistance::Log_Write(void) const +{ + // @LoggerMessage: SURF + // @Vehicles: Copter + // @Description: Surface distance measurement + // @Field: TimeUS: Time since system startup + // @Field: I: Instance + // @FieldBitmaskEnum: St: Surface_Distance_Status + // @Field: D: Raw Distance + // @Field: FD: Filtered Distance + // @Field: TO: Terrain Offset + + //Write to data flash log + AP::logger().WriteStreaming("SURF", + "TimeUS,I,St,D,FD,TO", + "s#-mmm", + "F--000", + "QBBfff", + AP_HAL::micros64(), + instance, + status, + (float)alt_cm * 0.01, + (float)alt_cm_filt.get() * 0.01, + (float)terrain_offset_cm * 0.01 + ); +} +#endif // HAL_LOGGING_ENABLED + +#endif // AP_RANGEFINDER_ENABLED + +bool AP_SurfaceDistance::data_stale(void) +{ + WITH_SEMAPHORE(sem); + return (AP_HAL::millis() - last_healthy_ms) > RANGEFINDER_TIMEOUT_MS; +} + +bool AP_SurfaceDistance::enabled_and_healthy(void) const +{ + return enabled && alt_healthy; +} diff --git a/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h new file mode 100644 index 0000000000000..cfbb5d2fe844c --- /dev/null +++ b/libraries/AP_SurfaceDistance/AP_SurfaceDistance.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include + +class AP_SurfaceDistance { +public: + AP_SurfaceDistance(Rotation rot, const AP_InertialNav& inav, uint8_t i) : rotation(rot), inertial_nav(inav), instance(i) {}; + + void update(); + + // check if the last healthy range finder reading is too old to be considered valid + bool data_stale(void); + + // helper to check that rangefinder was last reported as enabled and healthy + bool enabled_and_healthy(void) const; + + // get inertially interpolated rangefinder height + bool get_rangefinder_height_interpolated_cm(int32_t& ret) const; + + 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 + int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder + float inertial_alt_cm; // inertial alt at time of last rangefinder sample + LowPassFilterFloat alt_cm_filt {0.5}; // altitude filter + int16_t alt_cm_glitch_protected; // last glitch protected altitude + int8_t glitch_count; // non-zero number indicates rangefinder is glitching + uint32_t glitch_cleared_ms; // system time glitch cleared + float terrain_offset_cm; // filtered terrain offset (e.g. terrain's height above EKF origin) + +private: +#if HAL_LOGGING_ENABLED + void Log_Write(void) const; +#endif + + // multi-thread access support + HAL_Semaphore sem; + + const uint8_t instance; + uint8_t status; + uint32_t last_healthy_ms; + + const AP_InertialNav& inertial_nav; + const Rotation rotation; +}; From a67971d23744c105e6325cf19dd9bb274d5c0584 Mon Sep 17 00:00:00 2001 From: MattKear Date: Wed, 1 May 2024 13:50:58 +0100 Subject: [PATCH 2/2] Copter: use new surface distance library --- ArduCopter/Copter.h | 19 ++--- ArduCopter/config.h | 20 ----- ArduCopter/sensors.cpp | 125 ++++---------------------------- ArduCopter/surface_tracking.cpp | 5 +- ArduCopter/wscript | 1 + 5 files changed, 24 insertions(+), 146 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a872dc67a2888..026faaa7d5cd9 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -71,6 +71,7 @@ #include #include #include +#include // Configuration #include "defines.h" @@ -251,20 +252,10 @@ class Copter : public AP_Vehicle { AP_Int8 *flight_modes; const uint8_t num_flight_modes = 6; - struct RangeFinderState { - bool enabled:1; - bool alt_healthy:1; // true if we can trust the altitude from the rangefinder - int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder - float inertial_alt_cm; // inertial alt at time of last rangefinder sample - uint32_t last_healthy_ms; - LowPassFilterFloat alt_cm_filt; // altitude filter - int16_t alt_cm_glitch_protected; // last glitch protected altitude - int8_t glitch_count; // non-zero number indicates rangefinder is glitching - uint32_t glitch_cleared_ms; // system time glitch cleared - float terrain_offset_cm; // filtered terrain offset (e.g. terrain's height above EKF origin) - } rangefinder_state, rangefinder_up_state; - - // return rangefinder height interpolated using inertial altitude + AP_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, inertial_nav, 0U}; + AP_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, inertial_nav, 1U}; + + // helper function to get inertially interpolated rangefinder height. bool get_rangefinder_height_interpolated_cm(int32_t& ret) const; class SurfaceTracking { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6a1ef097ed311..a789c6ddd6e76 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -74,14 +74,6 @@ # define RANGEFINDER_ENABLED ENABLED #endif -#ifndef RANGEFINDER_HEALTH_MAX - # define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder -#endif - -#ifndef RANGEFINDER_TIMEOUT_MS -# define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second -#endif - #ifndef RANGEFINDER_FILT_DEFAULT # define RANGEFINDER_FILT_DEFAULT 0.5f // filter for rangefinder distance #endif @@ -90,18 +82,6 @@ # define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt #endif -#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF - # define RANGEFINDER_TILT_CORRECTION ENABLED -#endif - -#ifndef RANGEFINDER_GLITCH_ALT_CM - # define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch -#endif - -#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES - # define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading -#endif - #ifndef MAV_SYSTEM_ID # define MAV_SYSTEM_ID 1 #endif diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 589561b8cfe40..8cb09dcd934f0 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -10,7 +10,7 @@ void Copter::read_barometer(void) void Copter::init_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED +#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt); @@ -25,117 +25,31 @@ void Copter::init_rangefinder(void) // return rangefinder altitude in centimeters void Copter::read_rangefinder(void) { -#if RANGEFINDER_ENABLED == ENABLED +#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED rangefinder.update(); -#if RANGEFINDER_TILT_CORRECTION == ENABLED - const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); -#else - const float tilt_correction = 1.0f; -#endif - - // iterate through downward and upward facing lidar - struct { - RangeFinderState &state; - enum Rotation orientation; - } rngfnd[2] = {{rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}}; - - for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) { - // local variables to make accessing simpler - RangeFinderState &rf_state = rngfnd[i].state; - enum Rotation rf_orient = rngfnd[i].orientation; - - // update health - rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::Status::Good) && - (rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX)); - - // tilt corrected but unfiltered, not glitch protected alt - rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient); - - // remember inertial alt to allow us to interpolate rangefinder - rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm(); - - // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading - // are considered a glitch and glitch_count becomes non-zero - // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row. - // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset - const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected; - bool reset_terrain_offset = false; - if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { - rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1); - } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { - rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1); - } else { - rf_state.glitch_count = 0; - rf_state.alt_cm_glitch_protected = rf_state.alt_cm; - } - if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { - // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes - rf_state.glitch_count = 0; - rf_state.alt_cm_glitch_protected = rf_state.alt_cm; - rf_state.glitch_cleared_ms = AP_HAL::millis(); - reset_terrain_offset = true; - } - - // filter rangefinder altitude - uint32_t now = AP_HAL::millis(); - const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS; - if (rf_state.alt_healthy) { - if (timed_out) { - // reset filter if we haven't used it within the last second - rf_state.alt_cm_filt.reset(rf_state.alt_cm); - reset_terrain_offset = true; + rangefinder_state.update(); + rangefinder_up_state.update(); - } else { - rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f); - } - rf_state.last_healthy_ms = now; - } - - // handle reset of terrain offset - if (reset_terrain_offset) { - if (rf_orient == ROTATION_PITCH_90) { - // upward facing - rf_state.terrain_offset_cm = rf_state.inertial_alt_cm + rf_state.alt_cm; - } else { - // assume downward facing - rf_state.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm; - } - } - - // send downward facing lidar altitude and health to the libraries that require it #if HAL_PROXIMITY_ENABLED - if (rf_orient == ROTATION_PITCH_270) { - if (rangefinder_state.alt_healthy || timed_out) { - g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); - } - } -#endif + if (rangefinder_state.enabled_and_healthy() || rangefinder_state.data_stale()) { + g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); } +#endif -#else - // downward facing rangefinder - rangefinder_state.enabled = false; - rangefinder_state.alt_healthy = false; - rangefinder_state.alt_cm = 0; - - // upward facing rangefinder - rangefinder_up_state.enabled = false; - rangefinder_up_state.alt_healthy = false; - rangefinder_up_state.alt_cm = 0; #endif } // return true if rangefinder_alt can be used bool Copter::rangefinder_alt_ok() const { - return (rangefinder_state.enabled && rangefinder_state.alt_healthy); + return rangefinder_state.enabled_and_healthy(); } // return true if rangefinder_alt can be used bool Copter::rangefinder_up_ok() const { - return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy); + return rangefinder_up_state.enabled_and_healthy(); } // update rangefinder based terrain offset @@ -148,7 +62,7 @@ void Copter::update_rangefinder_terrain_offset() terrain_offset_cm = rangefinder_up_state.inertial_alt_cm + rangefinder_up_state.alt_cm_glitch_protected; rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_up_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt)); - if (rangefinder_state.alt_healthy || (AP_HAL::millis() - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS)) { + if (rangefinder_state.alt_healthy || rangefinder_state.data_stale()) { wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm); #if MODE_CIRCLE_ENABLED circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm); @@ -156,19 +70,12 @@ void Copter::update_rangefinder_terrain_offset() } } -/* - get inertially interpolated rangefinder height. Inertial height is - recorded whenever we update the rangefinder height, then we use the - difference between the inertial height at that time and the current - inertial height to give us interpolation of height from rangefinder - */ +// helper function to get inertially interpolated rangefinder height. bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const { - if (!rangefinder_alt_ok()) { - return false; - } - ret = rangefinder_state.alt_cm_filt.get(); - float inertial_alt_cm = inertial_nav.get_position_z_up_cm(); - ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm; - return true; +#if RANGEFINDER_ENABLED == ENABLED && AP_RANGEFINDER_ENABLED + return rangefinder_state.get_rangefinder_height_interpolated_cm(ret); +#else + return false; +#endif } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 99b05660fbb7d..57ceda03ef2d8 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -13,9 +13,8 @@ void Copter::SurfaceTracking::update_surface_offset() if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) || ((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) { - // calculate surfaces height above the EKF origin - // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m. curr_surface_alt_above_origin_cm is 7m (or 700cm) - RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; + // Get the appropriate surface distance state, the terrain offset is calculated in the surface distance lib. + AP_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; // update position controller target offset to the surface's alt above the EKF origin copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm); diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 13bde3905e896..08bec013e628a 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -27,6 +27,7 @@ def build(bld): 'AP_Devo_Telem', 'AC_AutoTune', 'AP_KDECAN', + 'AP_SurfaceDistance' ], )