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 with a 20m horizontal gate from the takeoff location if the 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();

    // 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 and XY position while still on
    // the ground without throttle up. Only the relative-to-takeoff
    // fallback consumes these; HAGL path ignores 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;
        _state.takeoff_pos_ne_valid = have_pos_ne;
    }

    // Pick the best available height
    // EKF's HAGL uses rangefinder or optflow AGL KF
    // fall back to height-since-takeoff and assume flat ground
    float height_m = 0;
    bool height_is_agl = ahrs.get_hagl(height_m);
    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 = 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: only
    //     trust the gate while still 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
    //   - 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