mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
AP_GroundEffect: add ground effect takeoff/touchdown detector library
Vehicle-agnostic detector that tells the EKF when to expect baro disturbance from rotor downwash near the ground, driving AP_AHRS set_takeoff_expected and set_touchdown_expected. Adds GNDEFF_ALT (altitude threshold and master enable) and GNDEFF_TMO (takeoff hold time).
This commit is contained in:
Andy Piper
Randy Mackay
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "AP_GroundEffect_config.h"
|
||||
|
||||
#if AP_GROUNDEFFECT_ENABLED
|
||||
|
||||
#include "AP_GroundEffect.h"
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Terrain/AP_Terrain.h>
|
||||
#include <AC_AttitudeControl/AC_PosControl.h>
|
||||
|
||||
// hard cap on the takeoff_expected window, irrespective of GNDEFF_TMO
|
||||
#define AP_GROUNDEFFECT_TAKEOFF_MAX_MS 5000U
|
||||
|
||||
// once we are using the relative-to-takeoff height fallback with GPS but
|
||||
// no rangefinder/terrain, disable the touchdown altitude gate once the
|
||||
// vehicle has drifted this far horizontally from where it lifted off, as
|
||||
// the terrain elevation under it may differ from the launch site
|
||||
#define AP_GROUNDEFFECT_TAKEOFF_DRIFT_MAX_M 20.0f
|
||||
|
||||
const AP_Param::GroupInfo AP_GroundEffect::var_info[] = {
|
||||
|
||||
// 1 was ENABLE; the master switch is folded into GNDEFF_ALT
|
||||
// (negative value disables the library)
|
||||
|
||||
// @Param: ALT
|
||||
// @DisplayName: Ground effect altitude threshold
|
||||
// @Description: Ground effect compensation altitude threshold. Compensation is turned off once the vehicle climbs this many meters above the takeoff location. Positive values cause compensation to be applied both during takeoff and landing. Zero keeps compensation enabled but removes the landing altitude gate, matching the legacy behaviour where any gentle descent counts. Negative values disable the feature. Altitude of the vehicle is derived from a downward facing rangefinder (if present), the terrain database (when available) or worst case using the height-change-since-takeoff assuming flat ground with a 20m horizontal gate disabling the touchdown side if the vehicle's horizontal position is available.
|
||||
// @Range: -1 10
|
||||
// @Units: m
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ALT", 2, AP_GroundEffect, _alt_m, 0.5),
|
||||
|
||||
// @Param: TMO
|
||||
// @DisplayName: Ground Effect Takeoff Timeout
|
||||
// @Description: Ground effect compensation timeout after takeoff. Compensation is turned off this many seconds after takeoff AND the vehicle has climbed at least GNDEFF_ALT. Compensation is also disabled after 5sec regardless of this timeout or the vehicle's altitude. Zero disables this timeout and only the altitude check is applied. Vehicles with strong baro disturbance from propwash should use values of 2 to 5 sec. This does not affect the compensation during touchdown.
|
||||
// @Range: 0 5
|
||||
// @Units: s
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TMO", 3, AP_GroundEffect, _timeout_s, 2),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_GroundEffect::AP_GroundEffect()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
void AP_GroundEffect::update(bool armed, bool land_complete, bool throttle_up)
|
||||
{
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
|
||||
if (is_negative(_alt_m) || !armed) {
|
||||
// disarmed or disabled (GNDEFF_ALT < 0) - clear state and tell EKF nothing is expected
|
||||
_state.takeoff_expected = false;
|
||||
_state.touchdown_expected = false;
|
||||
ahrs.set_takeoff_expected(false);
|
||||
ahrs.set_touchdown_expected(false);
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t tnow_ms = AP_HAL::millis();
|
||||
|
||||
// Vehicle allows the takeoff window: assert it. Normally this latches
|
||||
// while still on the ground (takeoff imminent); modes that spool up
|
||||
// after they are already airborne - e.g. THROW drop, where prop wash
|
||||
// contaminates the baro once the drop is detected - assert it in the
|
||||
// air. The release check below and the 5 s hard cap bound it either way.
|
||||
_state.takeoff_expected = _takeoff_comp_enabled;
|
||||
|
||||
// Anchor the takeoff timer, altitude and XY position while still on
|
||||
// the ground without throttle up. Only the relative-to-takeoff
|
||||
// fallback consumes these; HAGL / terrain paths ignore them.
|
||||
float pos_d_m = 0;
|
||||
UNUSED_RESULT(ahrs.get_relative_position_D_origin_float(pos_d_m));
|
||||
Vector2f pos_ne_m;
|
||||
const bool have_pos_ne = ahrs.get_relative_position_NE_origin_float(pos_ne_m);
|
||||
|
||||
if (!throttle_up && land_complete) {
|
||||
_state.takeoff_time_ms = tnow_ms;
|
||||
_state.takeoff_alt_m = -pos_d_m;
|
||||
_state.takeoff_pos_ne_m = pos_ne_m;
|
||||
}
|
||||
|
||||
// Pick the best available height. EKF HAGL covers rangefinder and
|
||||
// EKF3's optflow AGL KF; terrain database covers GPS + onboard tiles;
|
||||
// otherwise fall back to height-since-takeoff and assume flat ground.
|
||||
float height_m = 0;
|
||||
bool height_is_agl = ahrs.get_hagl(height_m);
|
||||
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
if (!height_is_agl) {
|
||||
AP_Terrain *terrain = AP::terrain();
|
||||
// extrapolate=false: with no tiles loaded, height_above_terrain
|
||||
// still "succeeds" by returning the raw AMSL altitude, which we
|
||||
// do not want; require real data.
|
||||
if (terrain != nullptr && terrain->height_above_terrain(height_m, false)) {
|
||||
height_is_agl = true;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
if (!height_is_agl) {
|
||||
height_m = -pos_d_m - _state.takeoff_alt_m;
|
||||
}
|
||||
|
||||
// GNDEFF_TMO is a minimum hold time before the altitude check is
|
||||
// allowed to release; the 5s hard timeout still applies unconditionally.
|
||||
const uint32_t min_hold_ms = MIN(uint32_t(_timeout_s * 1000.0f), AP_GROUNDEFFECT_TAKEOFF_MAX_MS);
|
||||
const bool above_alt = height_m > _alt_m;
|
||||
const bool min_hold_elapsed = AP_HAL::timeout_expired(_state.takeoff_time_ms, tnow_ms, min_hold_ms);
|
||||
const bool max_timeout = AP_HAL::timeout_expired(_state.takeoff_time_ms, tnow_ms, AP_GROUNDEFFECT_TAKEOFF_MAX_MS);
|
||||
|
||||
if (_state.takeoff_expected && (max_timeout || (min_hold_elapsed && above_alt))) {
|
||||
_state.takeoff_expected = false;
|
||||
}
|
||||
|
||||
// touchdown logic - slow horizontal motion AND slow descent AND near ground
|
||||
const bool ne_active = (_pos_control != nullptr) && _pos_control->NE_is_active();
|
||||
const bool d_active = (_pos_control != nullptr) && _pos_control->D_is_active();
|
||||
|
||||
const bool xy_speed_demand_low = ne_active && _pos_control->get_vel_target_NED_ms().xy().length() <= 1.25f;
|
||||
|
||||
Vector3f vel_ned_ms;
|
||||
const bool xy_speed_low = ahrs.get_velocity_NED(vel_ned_ms) && (vel_ned_ms.xy().length() < 1.25f);
|
||||
|
||||
const bool slow_horizontal = xy_speed_demand_low
|
||||
|| (xy_speed_low && !ne_active)
|
||||
|| _pilot_slow_horizontal;
|
||||
|
||||
const float target_climb_rate_ms = d_active ? _pos_control->get_vel_desired_U_ms() : 0.0f;
|
||||
const bool descent_demanded = d_active && target_climb_rate_ms < 0.0f;
|
||||
const bool slow_descent_demanded = descent_demanded && target_climb_rate_ms >= -1.0f;
|
||||
float vel_d_ms = 0;
|
||||
const bool speed_low_d = ahrs.get_velocity_D(vel_d_ms, _high_vibrations) && fabsf(vel_d_ms) <= 0.6f;
|
||||
const bool slow_descent = slow_descent_demanded || (speed_low_d && descent_demanded);
|
||||
|
||||
// Touchdown altitude gate.
|
||||
// - GNDEFF_ALT <= 0: legacy behaviour, any gentle descent counts
|
||||
// - HAGL or terrain height available: trust height_m directly
|
||||
// - relative-to-takeoff fallback with horizontal position: only
|
||||
// trust the gate while still within AP_GROUNDEFFECT_TAKEOFF_DRIFT_MAX_M
|
||||
// of the launch point; further out we cannot assume the ground
|
||||
// beneath us is at the takeoff elevation
|
||||
// - baro-only fallback (no horizontal position): assume flat ground
|
||||
bool near_ground;
|
||||
if (!is_positive(_alt_m)) {
|
||||
near_ground = true;
|
||||
} else if (height_is_agl || !have_pos_ne) {
|
||||
near_ground = height_m < _alt_m;
|
||||
} else {
|
||||
const float drift_m = (pos_ne_m - _state.takeoff_pos_ne_m).length();
|
||||
near_ground = (drift_m < AP_GROUNDEFFECT_TAKEOFF_DRIFT_MAX_M)
|
||||
&& (height_m < _alt_m);
|
||||
}
|
||||
|
||||
_state.touchdown_expected = slow_horizontal && slow_descent && near_ground;
|
||||
|
||||
ahrs.set_takeoff_expected(_state.takeoff_expected);
|
||||
ahrs.set_touchdown_expected(_state.touchdown_expected);
|
||||
}
|
||||
|
||||
#endif // AP_GROUNDEFFECT_ENABLED
|
||||
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// Vehicle-agnostic ground-effect takeoff/touchdown detector. Decides when
|
||||
// the EKF should be told to expect baro disturbance from rotor downwash
|
||||
// near the ground, and pushes that signal to AP_AHRS via
|
||||
// set_takeoff_expected / set_touchdown_expected. The vehicle wires an
|
||||
// AC_PosControl in once and the library queries it (and AP::ahrs())
|
||||
// directly each tick. Per-cycle vehicle state the takeoff window needs
|
||||
// is passed straight to update(); mode-conditional or sensor-derived
|
||||
// flags use setters so the vehicle only updates them when the underlying
|
||||
// signal actually changes.
|
||||
//
|
||||
// Two independent signals are emitted:
|
||||
//
|
||||
// takeoff_expected - latched true once the vehicle is armed and
|
||||
// land_complete, cleared once (a) GNDEFF_TMO has
|
||||
// elapsed AND height has cleared GNDEFF_ALT, or
|
||||
// (b) a 5 s hard timeout fires. GNDEFF_TMO
|
||||
// exists to keep compensation engaged through a
|
||||
// baro disturbance window even when the EKF
|
||||
// altitude has already crossed the threshold.
|
||||
//
|
||||
// touchdown_expected - re-evaluated every tick from slow horizontal
|
||||
// speed AND slow descent AND "near ground"
|
||||
// (defined by GNDEFF_ALT). Never latched: it is
|
||||
// the instantaneous answer to "does this look
|
||||
// like a landing approach right now?".
|
||||
//
|
||||
// Height source (used by both signals via "above GNDEFF_ALT" /
|
||||
// "below GNDEFF_ALT" checks) is selected in this order:
|
||||
//
|
||||
// 1. AP_AHRS::get_hagl() - rangefinder, or EKF3's
|
||||
// optflow AGL Kalman filter
|
||||
// 2. AP_Terrain::height_above_terrain()
|
||||
// - GPS position plus onboard
|
||||
// terrain tiles for current loc
|
||||
// 3. relative-to-takeoff (-pos_d minus the altitude latched at
|
||||
// takeoff) with horizontal position available
|
||||
// 4. relative-to-takeoff with no horizontal position (baro-only):
|
||||
// assumes the ground beneath the vehicle is at
|
||||
// the takeoff elevation
|
||||
//
|
||||
// Paths 3 and 4 are not strictly AGL: they trust that the ground has
|
||||
// not changed elevation since takeoff. For the takeoff_expected window
|
||||
// (which closes within ~5 s of takeoff) that is almost always fine.
|
||||
// For touchdown_expected (which the vehicle may evaluate minutes later,
|
||||
// hundreds of metres from launch) it is not, so path 3 additionally
|
||||
// requires the vehicle to be within
|
||||
// AP_GROUNDEFFECT_TAKEOFF_DRIFT_MAX_M of the takeoff XY position before
|
||||
// the touchdown altitude gate is allowed to fire. Drift further than
|
||||
// that and touchdown_expected stays false regardless of motion, since
|
||||
// we have no basis to believe the ground below is at takeoff elevation.
|
||||
// Path 4 (no horizontal position at all) cannot apply the drift gate
|
||||
// and has to assume flat terrain.
|
||||
//
|
||||
// GNDEFF_ALT carries three regimes:
|
||||
//
|
||||
// < 0 library entirely disabled, no signals emitted
|
||||
// == 0 library on; touchdown altitude gate disabled (matches the
|
||||
// legacy "any gentle descent counts" behaviour); takeoff window
|
||||
// relies on the 5 s hard cap (and GNDEFF_TMO if set)
|
||||
// > 0 library on with the threshold actively gating both sides
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_GroundEffect_config.h"
|
||||
|
||||
#if AP_GROUNDEFFECT_ENABLED
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
class AC_PosControl;
|
||||
|
||||
class AP_GroundEffect
|
||||
{
|
||||
public:
|
||||
AP_GroundEffect();
|
||||
|
||||
CLASS_NO_COPY(AP_GroundEffect);
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// Wire the position controller the library queries each tick. Called
|
||||
// once after the vehicle has allocated its AC_PosControl. The library
|
||||
// can not take the controller as a constructor argument because it
|
||||
// typically lives inside the vehicle's static parameter object, which
|
||||
// is built before AC_PosControl is allocated.
|
||||
void set_pos_control(const AC_PosControl &pos_control) { _pos_control = &pos_control; }
|
||||
|
||||
// Mode-conditional and sensor-derived signal setters. The vehicle
|
||||
// calls these when the underlying state changes (per-tick polling is
|
||||
// also fine).
|
||||
|
||||
// Enable ground effect compensation during takeoff.
|
||||
// Defaults to true, set to false in modes that disable ground effect compensation
|
||||
// (e.g. Copter's Throw mode)
|
||||
void enable_takeoff_comp(bool b) { _takeoff_comp_enabled = b; }
|
||||
|
||||
// mode-specific override for the slow-horizontal check
|
||||
// should be set to true in angle control modes (e.g AltHold) with angle target < 7.5deg
|
||||
void set_pilot_demanding_slow_horizontal(bool b) { _pilot_slow_horizontal = b; }
|
||||
|
||||
// vehicle's high-vibration flag, forwarded to AHRS get_velocity_D()
|
||||
void set_high_vibrations(bool b) { _high_vibrations = b; }
|
||||
|
||||
// Per-cycle entry point. Queries AHRS and the wired AC_PosControl,
|
||||
// runs the detector, and pushes set_takeoff_expected /
|
||||
// set_touchdown_expected to AP_AHRS. The three args are the basic
|
||||
// vehicle-state signals the takeoff window needs each tick:
|
||||
// armed - motors are armed
|
||||
// land_complete - vehicle is on the ground per the land detector
|
||||
// throttle_up - pilot is manually commanding throttle up (while
|
||||
// false the takeoff timer/altitude reference is held)
|
||||
void update(bool armed, bool land_complete, bool throttle_up);
|
||||
|
||||
private:
|
||||
AP_Float _alt_m; // altitude threshold; <0 disables the library entirely
|
||||
AP_Float _timeout_s; // minimum hold time before altitude check is allowed to release
|
||||
|
||||
const AC_PosControl *_pos_control;
|
||||
|
||||
// mode/sensor-derived inputs, updated via setters
|
||||
bool _takeoff_comp_enabled = true;
|
||||
bool _pilot_slow_horizontal;
|
||||
bool _high_vibrations;
|
||||
|
||||
struct {
|
||||
bool takeoff_expected;
|
||||
bool touchdown_expected;
|
||||
uint32_t takeoff_time_ms;
|
||||
float takeoff_alt_m;
|
||||
Vector2f takeoff_pos_ne_m; // EKF-origin XY at takeoff, used by the relative-to-takeoff fallback
|
||||
} _state;
|
||||
};
|
||||
|
||||
#endif // AP_GROUNDEFFECT_ENABLED
|
||||
@@ -0,0 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
#ifndef AP_GROUNDEFFECT_ENABLED
|
||||
#define AP_GROUNDEFFECT_ENABLED 1
|
||||
#endif
|
||||
Reference in New Issue
Block a user