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             : #pragma once
      17             : 
      18             : #include "AP_Landing_config.h"
      19             : 
      20             : #if HAL_LANDING_DEEPSTALL_ENABLED
      21             : 
      22             : #include <AP_Param/AP_Param.h>
      23             : #include <AP_Mission/AP_Mission.h>
      24             : #include <AP_Common/AP_Common.h>
      25             : #include <AP_Navigation/AP_Navigation.h>
      26             : #include <PID/PID.h>
      27             : 
      28             : class AP_Landing;
      29             : 
      30             : /// @class  AP_Landing_Deepstall
      31             : /// @brief  Class managing Plane Deepstall landing methods
      32             : class AP_Landing_Deepstall
      33             : {
      34             : private:
      35             :     friend class AP_Landing;
      36             : 
      37             :     // constructor
      38         692 :     AP_Landing_Deepstall(AP_Landing &_landing) :
      39         692 :         landing(_landing)
      40             :     {
      41         692 :         AP_Param::setup_object_defaults(this, var_info);
      42         692 :     }
      43             : 
      44             :     AP_Landing &landing;
      45             : 
      46             :     static const struct AP_Param::GroupInfo var_info[];
      47             : 
      48             :     AP_Float forward_speed;
      49             :     AP_Float slope_a;
      50             :     AP_Float slope_b;
      51             :     AP_Float approach_extension;
      52             :     AP_Float down_speed;
      53             :     AP_Float slew_speed;
      54             :     AP_Int16 elevator_pwm;
      55             :     AP_Float handoff_airspeed;
      56             :     AP_Float handoff_lower_limit_airspeed;
      57             :     AP_Float L1_period;
      58             :     AP_Float L1_i;
      59             :     AP_Float yaw_rate_limit;
      60             :     AP_Float time_constant;
      61             :     AP_Float min_abort_alt;
      62             :     AP_Float aileron_scalar;
      63             :     int32_t loiter_sum_cd;         // used for tracking the progress on loitering
      64             :     DEEPSTALL_STAGE stage;
      65             :     Location landing_point;
      66             :     Location extended_approach;
      67             :     Location breakout_location;
      68             :     Location arc;
      69             :     Location arc_entry;
      70             :     Location arc_exit;
      71             :     float target_heading_deg;      // target heading for the deepstall in degrees
      72             :     uint32_t stall_entry_time;     // time when the aircrafted enter the stall (in millis)
      73             :     uint16_t initial_elevator_pwm; // PWM to start slewing the elevator up from
      74             :     uint32_t last_time;            // last time the controller ran
      75             :     float L1_xtrack_i;             // L1 integrator for navigation
      76             :     PID ds_PID;
      77             :     int32_t last_target_bearing;   // used for tracking the progress on loitering
      78             :     float crosstrack_error; // current crosstrack error
      79             :     float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall
      80             :     bool hold_level; // locks the target yaw rate of the aircraft to 0, serves to hold the aircraft level at all times
      81             :     float approach_alt_offset;     // approach altitude offset
      82             : 
      83             :     //public AP_Landing interface
      84             :     void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
      85             :     void verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed);
      86             :     bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
      87             :             const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms,
      88             :             const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range);
      89             :     void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc,
      90             :             const Location &current_loc, int32_t &target_altitude_offset_cm);
      91             :     bool override_servos(void);
      92             :     bool request_go_around(void);
      93             :     bool get_target_altitude_location(Location &location);
      94             :     float get_target_airspeed_ms(void) const;
      95             :     bool is_throttle_suppressed(void) const;
      96             :     bool is_flying_forward(void) const;
      97             :     bool is_on_approach(void) const;
      98             :     bool terminate(void);
      99             :     void Log(void) const;
     100             : 
     101             :     bool send_deepstall_message(mavlink_channel_t chan) const;
     102             : 
     103             :     const AP_PIDInfo& get_pid_info(void) const;
     104             : 
     105             :     //private helpers
     106             :     void build_approach_path(bool use_current_heading);
     107             :     float predict_travel_distance(const Vector3f wind, const float height, const bool print);
     108             :     bool verify_breakout(const Location &current_loc, const Location &target_loc, const float height_error) const;
     109             :     float update_steering(void);
     110             : 
     111             :     #define DEEPSTALL_LOITER_ALT_TOLERANCE 5.0f
     112             : };
     113             : 
     114             : #endif  // HAL_LANDING_DEEPSTALL_ENABLED