ardupilot/libraries/AP_GroundEffect/AP_GroundEffect.cpp

/*
   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 <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, 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_NE_MAX_M 20.0f

const AP_Param::GroupInfo AP_GroundEffect::var_info[] = {

    // @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 altitude gating: the takeoff window is released once GNDEFF_TMO has elapsed and the vehicle has climbed at all, and any gentle descent counts as a landing (the legacy behaviour). Negative values disable the feature. Altitude of the vehicle is derived from a downward facing rangefinder (if present) or using the height-change-since-takeoff assuming flat ground and no baro drift. More than 20m from the takeoff location (when a horizontal position is available) the landing altitude gate is dropped and any gentle descent counts as a landing.
    // @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();

    // latch takeoff_expected while armed on the ground; the release check below clears it
    if (!_takeoff_comp_enabled) {
        _state.takeoff_expected = false;
    } else if (land_complete) {
        _state.takeoff_expected = true;
    }

    // Anchor the takeoff timer, altitude, XY position and HAGL while still
    // on the ground without throttle up.
    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);
    float hagl_m = 0;
    const bool height_is_agl = ahrs.get_hagl(hagl_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;
        _state.takeoff_pos_ne_valid = have_pos_ne;
        // HAGL is not zero on the ground, it reads the rangefinder ground clearance
        _state.takeoff_hagl_m = height_is_agl ? hagl_m : 0.0f;
    }

    // Pick the best available height
    // EKF's HAGL uses rangefinder or optflow AGL KF, measured from the on-ground reading
    // fall back to height-since-takeoff and assume flat ground
    float height_m;
    if (height_is_agl) {
        height_m = hagl_m - _state.takeoff_hagl_m;
    } else {
        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 = uint32_t(constrain_float(_timeout_s * 1000.0f, 0.0f, float(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: trust height_m directly
    //   - relative-to-takeoff fallback with horizontal position: gate only
    //     while within AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M of the launch
    //     point; further out we cannot assume the ground beneath us is at
    //     the takeoff elevation, so any gentle descent counts
    //   - 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 || !_state.takeoff_pos_ne_valid) {
        near_ground = height_m < _alt_m;
    } else {
        const float drift_ne_m = (pos_ne_m - _state.takeoff_pos_ne_m).length();
        near_ground = (drift_ne_m >= AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_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