From 79783470444acd3971525362242962188fe42fbe Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Mar 2023 11:31:34 +1030 Subject: [PATCH] Copter: Add support for terrain altitude time constant --- ArduCopter/Copter.cpp | 2 ++ ArduCopter/Copter.h | 3 +++ ArduCopter/Parameters.cpp | 8 ++++++++ ArduCopter/Parameters.h | 1 + ArduCopter/sensors.cpp | 28 ++++++++++++++++++++++------ ArduCopter/surface_tracking.cpp | 6 ++---- 6 files changed, 38 insertions(+), 10 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index d352fbd52c..e2ee660d47 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -137,6 +137,8 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { FAST_TASK(update_home_from_EKF), // check if we've landed or crashed FAST_TASK(update_land_and_crash_detectors), + // surface tracking update + FAST_TASK(update_rangefinder_terrain_offset), #if HAL_MOUNT_ENABLED // camera mount's fast update FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fa18d76d97..77b11873f4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -262,6 +262,7 @@ private: 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 @@ -269,6 +270,7 @@ private: class SurfaceTracking { public: + // update_surface_offset - manages the vertical offset of the position controller to follow the // measured ground or ceiling level measured using the range finder. void update_surface_offset(); @@ -900,6 +902,7 @@ private: void read_rangefinder(void); bool rangefinder_alt_ok() const; bool rangefinder_up_ok() const; + void update_rangefinder_terrain_offset(); void update_optical_flow(void); // takeoff_check.cpp diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 820e63c260..b15f9e42ff 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1200,6 +1200,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @User: Standard AP_GROUPINFO("PLDP_SPEED_DN", 4, ParametersG2, pldp_descent_speed_ms, 0.0), + // @Param: SURFTRAK_TC + // @DisplayName: Surface Tracking Filter Time Constant + // @Description: Time to achieve 63.2% of the surface altitude measurement change. If 0 filtering is disabled + // @Units: s + // @Range: 0 5 + // @User: Advanced + AP_GROUPINFO("SURFTRAK_TC", 5, ParametersG2, surftrak_tc, 1.0), + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 93f309ca73..dd6f6ddaa6 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -674,6 +674,7 @@ public: AP_Int8 surftrak_mode; AP_Int8 failsafe_dr_enable; AP_Int16 failsafe_dr_timeout; + AP_Float surftrak_tc; // ramp time of throttle during take-off AP_Float takeoff_throttle_slew_time; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index afa8c7e333..7f866940d5 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -74,6 +74,7 @@ void Copter::read_rangefinder(void) // 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.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm; rf_state.glitch_cleared_ms = AP_HAL::millis(); } @@ -84,6 +85,7 @@ void Copter::read_rangefinder(void) 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); + rf_state.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm; } else { rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f); } @@ -91,17 +93,13 @@ void Copter::read_rangefinder(void) } // 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) { - wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); -#if MODE_CIRCLE_ENABLED - circle_nav->set_rangefinder_alt(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); -#endif -#if HAL_PROXIMITY_ENABLED g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); -#endif } } +#endif } #else @@ -129,6 +127,24 @@ bool Copter::rangefinder_up_ok() const return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy); } +// update rangefinder based terrain offset +// terrain offset is the terrain's height above the EKF origin +void Copter::update_rangefinder_terrain_offset() +{ + float terrain_offset_cm = rangefinder_state.inertial_alt_cm - rangefinder_state.alt_cm_glitch_protected; + rangefinder_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt)); + + 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)) { + 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); +#endif + } +} + /* get inertially interpolated rangefinder height. Inertial height is recorded whenever we update the rangefinder height, then we use the diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 669e7c646a..99b05660fb 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -16,11 +16,9 @@ void Copter::SurfaceTracking::update_surface_offset() // 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; - const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; - const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get(); // update position controller target offset to the surface's alt above the EKF origin - copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm); + copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm); last_update_ms = now_ms; valid_for_logging = true; @@ -30,7 +28,7 @@ void Copter::SurfaceTracking::update_surface_offset() if (timeout || reset_target || (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) { - copter.pos_control->set_pos_offset_z_cm(curr_surface_alt_above_origin_cm); + copter.pos_control->set_pos_offset_z_cm(rf_state.terrain_offset_cm); reset_target = false; last_glitch_cleared_ms = rf_state.glitch_cleared_ms; }