mirror of https://github.com/ArduPilot/ardupilot
Copter: use new surface distance library
This commit is contained in:
MattKear
Andrew Tridgell
parent
0a6fa4f886
commit
ae38c96a04
|
|
@ -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"
|
||||
|
|
@ -251,20 +252,10 @@ private:
|
|||
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;
|
||||
AP_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, inertial_nav, 0U};
|
||||
AP_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, inertial_nav, 1U};
|
||||
|
||||
// return rangefinder height interpolated using inertial altitude
|
||||
// helper function to get inertially interpolated rangefinder height.
|
||||
bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
|
||||
|
||||
class SurfaceTracking {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
rangefinder_state.update();
|
||||
rangefinder_up_state.update();
|
||||
|
||||
// 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;
|
||||
|
||||
} 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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ def build(bld):
|
|||
'AP_Devo_Telem',
|
||||
'AC_AutoTune',
|
||||
'AP_KDECAN',
|
||||
'AP_SurfaceDistance'
|
||||
],
|
||||
)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue