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_Param/AP_Param.h>
19 : #include <AP_Mission/AP_Mission.h>
20 : #include <AP_Common/AP_Common.h>
21 : #include <AP_TECS/AP_TECS.h>
22 : #include <AP_Navigation/AP_Navigation.h>
23 : #include "AP_Landing_Deepstall.h"
24 : #include <AP_Common/Location.h>
25 :
26 : /// @class AP_Landing
27 : /// @brief Class managing ArduPlane landing methods
28 : class AP_Landing {
29 : friend class AP_Landing_Deepstall;
30 :
31 : public:
32 : FUNCTOR_TYPEDEF(set_target_altitude_proportion_fn_t, void, const Location&, float);
33 : FUNCTOR_TYPEDEF(constrain_target_altitude_location_fn_t, void, const Location&, const Location&);
34 : FUNCTOR_TYPEDEF(adjusted_altitude_cm_fn_t, int32_t);
35 : FUNCTOR_TYPEDEF(adjusted_relative_altitude_cm_fn_t, int32_t);
36 : FUNCTOR_TYPEDEF(disarm_if_autoland_complete_fn_t, void);
37 : FUNCTOR_TYPEDEF(update_flight_stage_fn_t, void);
38 :
39 : AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_TECS *_tecs_Controller, AP_Navigation *_nav_controller, AP_FixedWing &_aparm,
40 : set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn,
41 : constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn,
42 : adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn,
43 : adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn,
44 : disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn,
45 : update_flight_stage_fn_t _update_flight_stage_fn);
46 :
47 : /* Do not allow copies */
48 : CLASS_NO_COPY(AP_Landing);
49 :
50 :
51 : // NOTE: make sure to update is_type_valid()
52 : enum Landing_Type {
53 : TYPE_STANDARD_GLIDE_SLOPE = 0,
54 : #if HAL_LANDING_DEEPSTALL_ENABLED
55 : TYPE_DEEPSTALL = 1,
56 : #endif
57 : // TODO: TYPE_PARACHUTE,
58 : // TODO: TYPE_HELICAL,
59 : };
60 :
61 : // we support upto 32 boolean bits for users wanting to change landing behaviour.
62 : enum OptionsMask {
63 : ON_LANDING_FLARE_USE_THR_MIN = (1<<0), // If set then set trottle to thr_min instead of zero on final flare
64 : ON_LANDING_USE_ARSPD_MAX = (1<<1), // If set then allow landing throttle constraint to be increased from trim airspeed to max airspeed (AIRSPEED_MAX)
65 : };
66 :
67 : void convert_parameters(void);
68 :
69 : void reset(void);
70 :
71 : void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
72 : bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
73 : const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed);
74 : bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
75 : const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range);
76 : void adjust_landing_slope_for_rangefinder_bump(AP_FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm);
77 : void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm);
78 : bool override_servos(void);
79 : void check_if_need_to_abort(const AP_FixedWing::Rangefinder_State &rangefinder_state);
80 : bool request_go_around(void);
81 : bool is_flaring(void) const;
82 : bool is_on_final(void) const;
83 : bool is_on_approach(void) const;
84 : bool is_ground_steering_allowed(void) const;
85 : bool is_throttle_suppressed(void) const;
86 : bool is_flying_forward(void) const;
87 : bool use_thr_min_during_flare(void) const; //defaults to false, but _options bit zero enables it.
88 : bool allow_max_airspeed_on_land(void) const; //defaults to false, but _options bit one enables it.
89 : void handle_flight_stage_change(const bool _in_landing_stage);
90 : int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
91 : bool get_target_altitude_location(Location &location);
92 : bool send_landing_message(mavlink_channel_t chan);
93 :
94 : // terminate the flight with an immediate landing, returns false if unable to be used for termination
95 : bool terminate(void);
96 :
97 : // helper functions
98 : bool restart_landing_sequence(void);
99 : float get_target_airspeed_ms(void);
100 :
101 : // accessor functions for the params and states
102 : static const struct AP_Param::GroupInfo var_info[];
103 :
104 : // Return the landing type
105 : Landing_Type get_type() const { return (Landing_Type)type.get(); }
106 :
107 23256 : int16_t get_pitch_cd(void) const { return pitch_deg*100; }
108 : float get_flare_sec(void) const { return flare_sec; }
109 29586 : int8_t get_disarm_delay(void) const { return disarm_delay; }
110 8438671 : int8_t get_then_servos_neutral(void) const { return then_servos_neutral; }
111 35169 : int8_t get_abort_throttle_enable(void) const { return abort_throttle_enable; }
112 199061 : int8_t get_flap_percent(void) const { return flap_percent; }
113 3609062 : int16_t get_throttle_slewrate(void) const { return throttle_slewrate; }
114 35416 : bool is_commanded_go_around(void) const { return flags.commanded_go_around; }
115 : bool is_complete(void) const;
116 7 : void set_initial_slope(void) { initial_slope = slope; }
117 : bool is_expecting_impact(void) const;
118 : void Log(void) const;
119 : const class AP_PIDInfo * get_pid_info(void) const;
120 :
121 : // landing altitude offset (meters)
122 : float alt_offset;
123 :
124 : private:
125 : struct {
126 : // denotes if a go-around has been commanded for landing
127 : bool commanded_go_around:1;
128 :
129 : // are we in auto and flight_stage is LAND
130 : bool in_progress:1;
131 : } flags;
132 :
133 : AP_Int16 _options; // user-configurable bitmask options, via a parameter, for landing
134 :
135 : // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope
136 : float initial_slope;
137 :
138 : // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / prev_WP_loc.get_distance(next_WP_loc)
139 : float slope;
140 :
141 : float height_flare_log;
142 :
143 : AP_Mission &mission;
144 : AP_AHRS &ahrs;
145 : AP_TECS *tecs_Controller;
146 : AP_Navigation *nav_controller;
147 :
148 : AP_FixedWing &aparm;
149 :
150 : set_target_altitude_proportion_fn_t set_target_altitude_proportion_fn;
151 : constrain_target_altitude_location_fn_t constrain_target_altitude_location_fn;
152 : adjusted_altitude_cm_fn_t adjusted_altitude_cm_fn;
153 : adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn;
154 : disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
155 : update_flight_stage_fn_t update_flight_stage_fn;
156 :
157 : #if HAL_LANDING_DEEPSTALL_ENABLED
158 : // support for deepstall landings
159 : AP_Landing_Deepstall deepstall;
160 : #endif
161 :
162 : AP_Float pitch_deg;
163 : AP_Float flare_alt;
164 : AP_Float flare_sec;
165 : AP_Float pre_flare_airspeed;
166 : AP_Float pre_flare_alt;
167 : AP_Float pre_flare_sec;
168 : AP_Float slope_recalc_shallow_threshold;
169 : AP_Float slope_recalc_steep_threshold_to_abort;
170 : AP_Int8 disarm_delay;
171 : AP_Int8 then_servos_neutral;
172 : AP_Int8 abort_throttle_enable;
173 : AP_Int8 flap_percent;
174 : AP_Int16 throttle_slewrate;
175 : AP_Int8 type;
176 : AP_Int8 flare_effectivness_pct;
177 : AP_Float wind_comp;
178 :
179 : // Land Type STANDARD GLIDE SLOPE
180 :
181 : enum class SlopeStage {
182 : NORMAL = 0,
183 : APPROACH = 1,
184 : PREFLARE = 2,
185 : FINAL = 3,
186 : } type_slope_stage;
187 :
188 : struct {
189 : // once landed, post some landing statistics to the GCS
190 : bool post_stats:1;
191 :
192 : bool has_aborted_due_to_slope_recalc:1;
193 : } type_slope_flags;
194 :
195 : void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
196 : void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed);
197 : bool type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
198 : const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range);
199 :
200 : void type_slope_adjust_landing_slope_for_rangefinder_bump(AP_FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float wp_distance, int32_t &target_altitude_offset_cm);
201 :
202 : void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm);
203 : float type_slope_get_target_airspeed_ms(void);
204 : void type_slope_check_if_need_to_abort(const AP_FixedWing::Rangefinder_State &rangefinder_state);
205 : int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
206 : bool type_slope_request_go_around(void);
207 : void type_slope_log(void) const;
208 : bool type_slope_is_complete(void) const;
209 : bool type_slope_is_flaring(void) const;
210 : bool type_slope_is_on_final(void) const;
211 : bool type_slope_is_on_approach(void) const;
212 : bool type_slope_is_expecting_impact(void) const;
213 : bool type_slope_is_throttle_suppressed(void) const;
214 :
215 : // return a location alt in cm as AMSL
216 : int32_t loc_alt_AMSL_cm(const Location &loc) const;
217 : };
|
