Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Add support for terrain altitude time constant

This commit is contained in:
Leonard Hall 2023-03-04 11:31:34 +10:30 committed by Peter Barker
parent 6344faeb29
commit 7978347044
6 changed files with 38 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

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

3
ArduCopter/Copter.h
View File

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

8
ArduCopter/Parameters.cpp
View File

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

1
ArduCopter/Parameters.h
View File

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

28
ArduCopter/sensors.cpp
View File

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

6
ArduCopter/surface_tracking.cpp
View File

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