Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Start Surface Distance Library #26933

Merged
merged 2 commits into from
May 27, 2024
Merged

Start Surface Distance Library #26933

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bb431b7
AP_SurfaceDistance: Start library for tracking the floor/roof distance
MattKear May 1, 2024
a67971d
Copter: use new surface distance library
MattKear May 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 5 additions & 14 deletions ArduCopter/Copter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@
#include <AC_PrecLand/AC_PrecLand_config.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_Winch/AP_Winch_config.h>
#include <AP_SurfaceDistance/AP_SurfaceDistance.h>

// Configuration
#include "defines.h"
Expand Down Expand Up @@ -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};
MattKear marked this conversation as resolved.
Show resolved Hide resolved

// helper function to get inertially interpolated rangefinder height.
bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;

class SurfaceTracking {
Expand Down
20 changes: 0 additions & 20 deletions ArduCopter/config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
125 changes: 16 additions & 109 deletions ArduCopter/sensors.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -148,27 +62,20 @@ 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);
#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
*/
// 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
}
5 changes: 2 additions & 3 deletions ArduCopter/surface_tracking.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions ArduCopter/wscript
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ def build(bld):
'AP_Devo_Telem',
'AC_AutoTune',
'AP_KDECAN',
'AP_SurfaceDistance'
],
)

Expand Down
Loading
Loading