Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_GroundEffect_config.h"
      17             : 
      18             : #if AP_GROUNDEFFECT_ENABLED
      19             : 
      20             : #include "AP_GroundEffect.h"
      21             : #include <AP_AHRS/AP_AHRS.h>
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AC_AttitudeControl/AC_PosControl.h>
      24             : 
      25             : // hard cap on the takeoff_expected window, irrespective of GNDEFF_TMO
      26             : #define AP_GROUNDEFFECT_TAKEOFF_MAX_MS 5000U
      27             : 
      28             : // once we are using the relative-to-takeoff height fallback with GPS but
      29             : // no rangefinder, disable the touchdown altitude gate once the
      30             : // vehicle has drifted this far horizontally from where it lifted off, as
      31             : // the terrain elevation under it may differ from the launch site
      32             : #define AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M 20.0f
      33             : 
      34             : const AP_Param::GroupInfo AP_GroundEffect::var_info[] = {
      35             : 
      36             :     // @Param: ALT
      37             :     // @DisplayName: Ground effect altitude threshold
      38             :     // @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.
      39             :     // @Range: -1 10
      40             :     // @Units: m
      41             :     // @User: Advanced
      42             :     AP_GROUPINFO("ALT", 2, AP_GroundEffect, _alt_m, 0.5),
      43             : 
      44             :     // @Param: TMO
      45             :     // @DisplayName: Ground Effect Takeoff Timeout
      46             :     // @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.
      47             :     // @Range: 0 5
      48             :     // @Units: s
      49             :     // @User: Advanced
      50             :     AP_GROUPINFO("TMO", 3, AP_GroundEffect, _timeout_s, 2),
      51             : 
      52             :     AP_GROUPEND
      53             : };
      54             : 
      55        1001 : AP_GroundEffect::AP_GroundEffect()
      56             : {
      57        1001 :     AP_Param::setup_object_defaults(this, var_info);
      58        1001 : }
      59             : 
      60     3800955 : void AP_GroundEffect::update(bool armed, bool land_complete, bool throttle_up)
      61             : {
      62     3800955 :     AP_AHRS &ahrs = AP::ahrs();
      63             : 
      64     3800955 :     if (is_negative(_alt_m) || !armed) {
      65             :         // disarmed or disabled (GNDEFF_ALT < 0) - clear state and tell EKF nothing is expected
      66     1332775 :         _state.takeoff_expected = false;
      67     1332775 :         _state.touchdown_expected = false;
      68     1332775 :         ahrs.set_takeoff_expected(false);
      69     1332775 :         ahrs.set_touchdown_expected(false);
      70     1332775 :         return;
      71             :     }
      72             : 
      73     2468180 :     const uint32_t tnow_ms = AP_HAL::millis();
      74             : 
      75             :     // latch takeoff_expected while armed on the ground; the release check below clears it
      76     2468180 :     if (!_takeoff_comp_enabled) {
      77        4043 :         _state.takeoff_expected = false;
      78     2464137 :     } else if (land_complete) {
      79      164471 :         _state.takeoff_expected = true;
      80             :     }
      81             : 
      82             :     // Anchor the takeoff timer, altitude, XY position and HAGL while still
      83             :     // on the ground without throttle up.
      84     2468180 :     float pos_d_m = 0;
      85     2468180 :     UNUSED_RESULT(ahrs.get_relative_position_D_origin_float(pos_d_m));
      86     2468180 :     Vector2f pos_ne_m;
      87     2468180 :     const bool have_pos_ne = ahrs.get_relative_position_NE_origin_float(pos_ne_m);
      88     2468180 :     float hagl_m = 0;
      89     2468180 :     const bool height_is_agl = ahrs.get_hagl(hagl_m);
      90             : 
      91     2468180 :     if (!throttle_up && land_complete) {
      92      160200 :         _state.takeoff_time_ms = tnow_ms;
      93      160200 :         _state.takeoff_alt_m = -pos_d_m;
      94      160200 :         _state.takeoff_pos_ne_m = pos_ne_m;
      95      160200 :         _state.takeoff_pos_ne_valid = have_pos_ne;
      96             :         // HAGL is not zero on the ground, it reads the rangefinder ground clearance
      97      160200 :         _state.takeoff_hagl_m = height_is_agl ? hagl_m : 0.0f;
      98             :     }
      99             : 
     100             :     // Pick the best available height
     101             :     // EKF's HAGL uses rangefinder or optflow AGL KF, measured from the on-ground reading
     102             :     // fall back to height-since-takeoff and assume flat ground
     103             :     float height_m;
     104     2468180 :     if (height_is_agl) {
     105      290279 :         height_m = hagl_m - _state.takeoff_hagl_m;
     106             :     } else {
     107     2177901 :         height_m = -pos_d_m - _state.takeoff_alt_m;
     108             :     }
     109             : 
     110             :     // GNDEFF_TMO is a minimum hold time before the altitude check is
     111             :     // allowed to release; the 5s hard timeout still applies unconditionally.
     112     2468180 :     const uint32_t min_hold_ms = uint32_t(constrain_float(_timeout_s * 1000.0f, 0.0f, float(AP_GROUNDEFFECT_TAKEOFF_MAX_MS)));
     113     2468180 :     const bool above_alt = height_m > _alt_m;
     114     2468180 :     const bool min_hold_elapsed = AP_HAL::timeout_expired(_state.takeoff_time_ms, tnow_ms, min_hold_ms);
     115     2468180 :     const bool max_timeout = AP_HAL::timeout_expired(_state.takeoff_time_ms, tnow_ms, AP_GROUNDEFFECT_TAKEOFF_MAX_MS);
     116             : 
     117     2468180 :     if (_state.takeoff_expected && (max_timeout || (min_hold_elapsed && above_alt))) {
     118         535 :         _state.takeoff_expected = false;
     119             :     }
     120             : 
     121             :     // touchdown logic - slow horizontal motion AND slow descent AND near ground
     122     2468180 :     const bool ne_active = (_pos_control != nullptr) && _pos_control->NE_is_active();
     123     2468180 :     const bool d_active  = (_pos_control != nullptr) && _pos_control->D_is_active();
     124             : 
     125     2468180 :     const bool xy_speed_demand_low = ne_active && _pos_control->get_vel_target_NED_ms().xy().length() <= 1.25f;
     126             : 
     127     2468180 :     Vector3f vel_ned_ms;
     128     2468180 :     const bool xy_speed_low = ahrs.get_velocity_NED(vel_ned_ms) && (vel_ned_ms.xy().length() < 1.25f);
     129             : 
     130     2468180 :     const bool slow_horizontal = xy_speed_demand_low
     131      962938 :                                  || (xy_speed_low && !ne_active)
     132     3431118 :                                  || _pilot_slow_horizontal;
     133             : 
     134     2468180 :     const float target_climb_rate_ms = d_active ? _pos_control->get_vel_desired_U_ms() : 0.0f;
     135     2468180 :     const bool descent_demanded = d_active && target_climb_rate_ms < 0.0f;
     136     2468180 :     const bool slow_descent_demanded = descent_demanded && target_climb_rate_ms >= -1.0f;
     137     2468180 :     float vel_d_ms = 0;
     138     2468180 :     const bool speed_low_d = ahrs.get_velocity_D(vel_d_ms, _high_vibrations) && fabsf(vel_d_ms) <= 0.6f;
     139     2468180 :     const bool slow_descent = slow_descent_demanded || (speed_low_d && descent_demanded);
     140             : 
     141             :     // Touchdown altitude gate.
     142             :     //   - GNDEFF_ALT <= 0: legacy behaviour, any gentle descent counts
     143             :     //   - HAGL: trust height_m directly
     144             :     //   - relative-to-takeoff fallback with horizontal position: gate only
     145             :     //     while within AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M of the launch
     146             :     //     point; further out we cannot assume the ground beneath us is at
     147             :     //     the takeoff elevation, so any gentle descent counts
     148             :     //   - baro-only fallback (no horizontal position): assume flat ground
     149             :     bool near_ground;
     150     2468180 :     if (!is_positive(_alt_m)) {
     151           0 :         near_ground = true;
     152     2468180 :     } else if (height_is_agl || !have_pos_ne || !_state.takeoff_pos_ne_valid) {
     153      305921 :         near_ground = height_m < _alt_m;
     154             :     } else {
     155     2162259 :         const float drift_ne_m = (pos_ne_m - _state.takeoff_pos_ne_m).length();
     156     2162259 :         near_ground = (drift_ne_m >= AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M)
     157     2162259 :                       || (height_m < _alt_m);
     158             :     }
     159             : 
     160     2468180 :     _state.touchdown_expected = slow_horizontal && slow_descent && near_ground;
     161             : 
     162     2468180 :     ahrs.set_takeoff_expected(_state.takeoff_expected);
     163     2468180 :     ahrs.set_touchdown_expected(_state.touchdown_expected);
     164             : }
     165             : 
     166             : #endif  // AP_GROUNDEFFECT_ENABLED