Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: added rangefinder height interpolated using inertial alt

Browse Source 
this smooths rangefinder heights and allows for good estimated for
precision landing even with loss of some rangefinder samples during landing
This commit is contained in:
Andrew Tridgell 2020-02-19 09:22:31 +11:00 committed by Randy Mackay
parent 0b01bb531d
commit dd5fafe30e
4 changed files with 29 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.h
View File

@ -261,6 +261,7 @@ private:
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
@ -268,6 +269,11 @@ private:
uint32_t glitch_cleared_ms; // system time glitch cleared
} rangefinder_state, rangefinder_up_state;
/*
return rangefinder height interpolated using inertial altitude
*/
bool get_rangefinder_height_interpolated_cm(int32_t& ret);
class SurfaceTracking {
public:
// get desired climb rate (in cm/s) to achieve surface tracking

4
ArduCopter/mode.cpp
View File

@ -509,9 +509,7 @@ void Mode::make_safe_spool_down()
int32_t Mode::get_alt_above_ground_cm(void)
{
int32_t alt_above_ground;
if (copter.rangefinder_alt_ok()) {
alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
} else {
if (!copter.get_rangefinder_height_interpolated_cm(alt_above_ground)) {
bool navigating = pos_control->is_active_xy();
if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = copter.current_loc.alt;

4
ArduCopter/mode_rtl.cpp
View File

@ -461,9 +461,7 @@ void ModeRTL::compute_return_target()
// set curr_alt and return_target.alt from range finder
if (alt_type == ReturnTargetAltType::RETURN_TARGET_ALTTYPE_RANGEFINDER) {
if (copter.rangefinder_state.alt_healthy) {
// set curr_alt based on rangefinder altitude
curr_alt = copter.rangefinder_state.alt_cm_filt.get();
if (copter.get_rangefinder_height_interpolated_cm(curr_alt)) {
// set return_target.alt
rtl_path.return_target.set_alt_cm(MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)), Location::AltFrame::ABOVE_TERRAIN);
} else {

21
ArduCopter/sensors.cpp
View File

@ -54,6 +54,9 @@ void Copter::read_rangefinder(void)
// 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_altitude();
// 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.
@ -120,6 +123,24 @@ bool Copter::rangefinder_up_ok()
return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
}
/*
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 Copter::get_rangefinder_height_interpolated_cm(int32_t& ret)
{
if (!rangefinder_alt_ok()) {
return false;
}
ret = rangefinder_state.alt_cm_filt.get();
float inertial_alt_cm = inertial_nav.get_altitude();
ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm;
return true;
}
/*
update RPM sensors
*/