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             : // Vehicle-agnostic ground-effect takeoff/touchdown detector. Decides when
      17             : // the EKF should be told to expect baro disturbance from rotor downwash
      18             : // near the ground, and pushes that signal to AP_AHRS via
      19             : // set_takeoff_expected / set_touchdown_expected. The vehicle wires an
      20             : // AC_PosControl in once and the library queries it (and AP::ahrs())
      21             : // directly each tick. Per-cycle vehicle state the takeoff window needs
      22             : // is passed straight to update(); mode-conditional or sensor-derived
      23             : // flags use setters so the vehicle only updates them when the underlying
      24             : // signal actually changes.
      25             : //
      26             : // Two independent signals are emitted:
      27             : //
      28             : //   takeoff_expected   - latched true once the vehicle is armed and
      29             : //                        land_complete, cleared once (a) GNDEFF_TMO has
      30             : //                        elapsed AND height has cleared GNDEFF_ALT, or
      31             : //                        (b) a 5 s hard timeout fires. GNDEFF_TMO
      32             : //                        exists to keep compensation engaged through a
      33             : //                        baro disturbance window even when the EKF
      34             : //                        altitude has already crossed the threshold.
      35             : //
      36             : //   touchdown_expected - re-evaluated every tick from slow horizontal
      37             : //                        speed AND slow descent AND "near ground"
      38             : //                        (defined by GNDEFF_ALT). Never latched: it is
      39             : //                        the instantaneous answer to "does this look
      40             : //                        like a landing approach right now?".
      41             : //
      42             : // Height source (used by both signals via "above GNDEFF_ALT" /
      43             : // "below GNDEFF_ALT" checks) is selected in this order:
      44             : //
      45             : //   1. AP_AHRS::get_hagl()             - rangefinder, or EKF3's
      46             : //                                        optflow AGL Kalman filter
      47             : //   2. relative-to-takeoff (-pos_d minus the altitude latched at
      48             : //                          takeoff) with horizontal position available
      49             : //   3. relative-to-takeoff with no horizontal position (baro-only):
      50             : //                          assumes the ground beneath the vehicle is at
      51             : //                          the takeoff elevation
      52             : //
      53             : // Paths 2 and 3 are not strictly AGL: they trust that the ground has
      54             : // not changed elevation since takeoff. For the takeoff_expected window
      55             : // (which closes within ~5 s of takeoff) that is almost always fine.
      56             : // For touchdown_expected (which the vehicle may evaluate minutes later,
      57             : // hundreds of metres from launch) it is not, so path 2 additionally
      58             : // requires the vehicle to be within
      59             : // AP_GROUNDEFFECT_TAKEOFF_DRIFT_NE_MAX_M of the takeoff XY position before
      60             : // the touchdown altitude gate is allowed to fire. Drift further than
      61             : // that and touchdown_expected stays false regardless of motion, since
      62             : // we have no basis to believe the ground below is at takeoff elevation.
      63             : // Path 3 (no horizontal position at all) cannot apply the drift gate
      64             : // and has to assume flat terrain.
      65             : //
      66             : // GNDEFF_ALT carries three regimes:
      67             : //
      68             : //   <  0   library entirely disabled, no signals emitted
      69             : //   == 0   library on with no altitude gating: the takeoff window
      70             : //          releases once GNDEFF_TMO has elapsed and the vehicle has
      71             : //          climbed at all, and any gentle descent counts as a landing
      72             : //          (the legacy behaviour)
      73             : //   >  0   library on with the threshold actively gating both sides
      74             : 
      75             : #pragma once
      76             : 
      77             : #include "AP_GroundEffect_config.h"
      78             : 
      79             : #if AP_GROUNDEFFECT_ENABLED
      80             : 
      81             : #include <AP_Param/AP_Param.h>
      82             : #include <AP_Math/AP_Math.h>
      83             : 
      84             : class AC_PosControl;
      85             : 
      86             : class AP_GroundEffect
      87             : {
      88             : public:
      89             :     AP_GroundEffect();
      90             : 
      91             :     CLASS_NO_COPY(AP_GroundEffect);
      92             : 
      93             :     static const struct AP_Param::GroupInfo var_info[];
      94             : 
      95             :     // Wire the position controller the library queries each tick. Called
      96             :     // once after the vehicle has allocated its AC_PosControl. The library
      97             :     // can not take the controller as a constructor argument because it
      98             :     // typically lives inside the vehicle's static parameter object, which
      99             :     // is built before AC_PosControl is allocated.
     100        1001 :     void set_pos_control(const AC_PosControl &pos_control) { _pos_control = &pos_control; }
     101             : 
     102             :     // Mode-conditional and sensor-derived signal setters. The vehicle
     103             :     // calls these when the underlying state changes (per-tick polling is
     104             :     // also fine).
     105             : 
     106             :     // Enable ground effect compensation during takeoff.
     107             :     // Defaults to true, set to false in modes that disable ground effect compensation
     108             :     // (e.g. Copter's Throw mode)
     109     3800955 :     void enable_takeoff_comp(bool b) { _takeoff_comp_enabled = b; }
     110             : 
     111             :     // mode-specific override for the slow-horizontal check
     112             :     // should be set to true in angle control modes (e.g AltHold) with angle target < 7.5deg
     113     3800955 :     void set_pilot_demanding_slow_horizontal(bool b) { _pilot_slow_horizontal = b; }
     114             : 
     115             :     // vehicle's high-vibration flag, forwarded to AHRS get_velocity_D()
     116     3800955 :     void set_high_vibrations(bool b) { _high_vibrations = b; }
     117             : 
     118             :     // Per-cycle entry point. Queries AHRS and the wired AC_PosControl,
     119             :     // runs the detector, and pushes set_takeoff_expected /
     120             :     // set_touchdown_expected to AP_AHRS. The three args are the basic
     121             :     // vehicle-state signals the takeoff window needs each tick:
     122             :     //   armed         - motors are armed
     123             :     //   land_complete - vehicle is on the ground per the land detector
     124             :     //   throttle_up   - pilot is manually commanding throttle up (while
     125             :     //                   false the takeoff timer/altitude reference is held)
     126             :     void update(bool armed, bool land_complete, bool throttle_up);
     127             : 
     128             : private:
     129             :     AP_Float _alt_m;       // altitude threshold; <0 disables the library entirely
     130             :     AP_Float _timeout_s;   // minimum hold time before altitude check is allowed to release
     131             : 
     132             :     const AC_PosControl *_pos_control;
     133             : 
     134             :     // mode/sensor-derived inputs, updated via setters
     135             :     bool _takeoff_comp_enabled = true;
     136             :     bool _pilot_slow_horizontal;
     137             :     bool _high_vibrations;
     138             : 
     139             :     struct {
     140             :         bool     takeoff_expected;
     141             :         bool     touchdown_expected;
     142             :         uint32_t takeoff_time_ms;
     143             :         float    takeoff_alt_m;
     144             :         float    takeoff_hagl_m;    // HAGL while on the ground, 0 if HAGL was unavailable
     145             :         Vector2f takeoff_pos_ne_m;  // EKF-origin XY at takeoff, used by the relative-to-takeoff fallback
     146             :         bool     takeoff_pos_ne_valid;
     147             :     } _state;
     148             : };
     149             : 
     150             : #endif  // AP_GROUNDEFFECT_ENABLED