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 : /*
17 : * AP_Landing.cpp - Landing logic handler for ArduPlane
18 : */
19 :
20 : #include "AP_Landing.h"
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <AC_Fence/AC_Fence.h>
24 : #include <AP_InternalError/AP_InternalError.h>
25 :
26 : // table of user settable parameters
27 : const AP_Param::GroupInfo AP_Landing::var_info[] = {
28 :
29 : // @Param: SLOPE_RCALC
30 : // @DisplayName: Landing slope re-calc threshold
31 : // @Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your altitude is lower than the intended slope path. This value is the threshold of the correction to re-calculate the landing approach slope. Set to zero to keep the original slope all the way down and any detected baro drift will be corrected by pitching/throttling up to snap back to resume the original slope path. Otherwise, when a rangefinder altitude correction exceeds this threshold it will trigger a slope re-calculate to give a shallower slope. This also smoothes out the approach when flying over objects such as trees. Recommend a value of 2m.
32 : // @Range: 0 5
33 : // @Units: m
34 : // @Increment: 0.5
35 : // @User: Advanced
36 : AP_GROUPINFO("SLOPE_RCALC", 1, AP_Landing, slope_recalc_shallow_threshold, 2.0f),
37 :
38 : // @Param: ABORT_DEG
39 : // @DisplayName: Landing auto-abort slope threshold
40 : // @Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your actual altitude is higher than the intended slope path. Normally it would pitch down steeply but that can result in a crash with high airspeed so this allows remembering the baro offset and self-abort the landing and come around for another landing with the correct baro offset applied for a perfect slope. An auto-abort go-around will only happen once, next attempt will not auto-abort again. This operation happens entirely automatically in AUTO mode. This value is the delta degrees threshold to trigger the go-around compared to the original slope. Example: if set to 5 deg and the mission planned slope is 15 deg then if the new slope is 21 then it will go-around. Set to 0 to disable. Requires LAND_SLOPE_RCALC > 0.
41 : // @Range: 0 90
42 : // @Units: deg
43 : // @Increment: 0.1
44 : // @User: Advanced
45 : AP_GROUPINFO("ABORT_DEG", 2, AP_Landing, slope_recalc_steep_threshold_to_abort, 0),
46 :
47 : // @Param: PITCH_DEG
48 : // @DisplayName: Landing Pitch
49 : // @Description: Used in autoland to give the minimum pitch in the final stage of landing (after the flare). This parameter can be used to ensure that the final landing attitude is appropriate for the type of undercarriage on the aircraft. Note that it is a minimum pitch only - the landing code will control pitch above this value to try to achieve the configured landing sink rate.
50 : // @Units: deg
51 : // @Range: -20 20
52 : // @Increment: 10
53 : // @User: Advanced
54 : AP_GROUPINFO("PITCH_DEG", 3, AP_Landing, pitch_deg, 0),
55 :
56 : // @Param: FLARE_ALT
57 : // @DisplayName: Landing flare altitude
58 : // @Description: Altitude in autoland at which to lock heading and flare to the LAND_PITCH_DEG pitch. Note that this option is secondary to LAND_FLARE_SEC. For a good landing it preferable that the flare is triggered by LAND_FLARE_SEC.
59 : // @Units: m
60 : // @Range: 0 30
61 : // @Increment: 0.1
62 : // @User: Advanced
63 : AP_GROUPINFO("FLARE_ALT", 4, AP_Landing, flare_alt, 3.0f),
64 :
65 : // @Param: FLARE_SEC
66 : // @DisplayName: Landing flare time
67 : // @Description: Vertical time before landing point at which to lock heading and flare with the motor stopped. This is vertical time, and is calculated based solely on the current height above the ground and the current descent rate. Set to 0 if you only wish to flare based on altitude (see LAND_FLARE_ALT).
68 : // @Units: s
69 : // @Range: 0 10
70 : // @Increment: 0.1
71 : // @User: Advanced
72 : AP_GROUPINFO("FLARE_SEC", 5, AP_Landing, flare_sec, 2.0f),
73 :
74 : // @Param: PF_ALT
75 : // @DisplayName: Landing pre-flare altitude
76 : // @Description: Altitude to trigger pre-flare flight stage where LAND_PF_ARSPD controls airspeed. The pre-flare flight stage trigger works just like LAND_FLARE_ALT but higher. Disabled when LAND_PF_ARSPD is 0.
77 : // @Units: m
78 : // @Range: 0 30
79 : // @Increment: 0.1
80 : // @User: Advanced
81 : AP_GROUPINFO("PF_ALT", 6, AP_Landing, pre_flare_alt, 10.0f),
82 :
83 : // @Param: PF_SEC
84 : // @DisplayName: Landing pre-flare time
85 : // @Description: Vertical time to ground to trigger pre-flare flight stage where LAND_PF_ARSPD controls airspeed. This pre-flare flight stage trigger works just like LAND_FLARE_SEC but earlier. Disabled when LAND_PF_ARSPD is 0.
86 : // @Units: s
87 : // @Range: 0 10
88 : // @Increment: 0.1
89 : // @User: Advanced
90 : AP_GROUPINFO("PF_SEC", 7, AP_Landing, pre_flare_sec, 6.0f),
91 :
92 : // @Param: PF_ARSPD
93 : // @DisplayName: Landing pre-flare airspeed
94 : // @Description: Desired airspeed during pre-flare flight stage. This is useful to reduce airspeed just before the flare. Use 0 to disable.
95 : // @Units: m/s
96 : // @Range: 0 30
97 : // @Increment: 0.1
98 : // @User: Advanced
99 : AP_GROUPINFO("PF_ARSPD", 8, AP_Landing, pre_flare_airspeed, 0),
100 :
101 : // @Param: THR_SLEW
102 : // @DisplayName: Landing throttle slew rate
103 : // @Description: This parameter sets the slew rate for the throttle during auto landing. When this is zero the THR_SLEWRATE parameter is used during landing. The value is a percentage throttle change per second, so a value of 20 means to advance the throttle over 5 seconds on landing. Values below 50 are not recommended as it may cause a stall when airspeed is low and you can not throttle up fast enough.
104 : // @Units: %/s
105 : // @Range: 0 500
106 : // @Increment: 1
107 : // @User: Standard
108 : AP_GROUPINFO("THR_SLEW", 9, AP_Landing, throttle_slewrate, 0),
109 :
110 : // @Param: DISARMDELAY
111 : // @DisplayName: Landing disarm delay
112 : // @Description: After a landing has completed using a LAND waypoint, automatically disarm after this many seconds have passed. Use 0 to not disarm.
113 : // @Units: s
114 : // @Increment: 1
115 : // @Range: 0 127
116 : // @User: Advanced
117 : AP_GROUPINFO("DISARMDELAY", 10, AP_Landing, disarm_delay, 20),
118 :
119 : // @Param: THEN_NEUTRL
120 : // @DisplayName: Set servos to neutral after landing
121 : // @Description: When enabled, after an autoland and auto-disarm via LAND_DISARMDELAY happens then set all servos to neutral. This is helpful when an aircraft has a rough landing upside down or a crazy angle causing the servos to strain.
122 : // @Values: 0:Disabled, 1:Servos to Neutral, 2:Servos to Zero PWM
123 : // @User: Advanced
124 : AP_GROUPINFO("THEN_NEUTRL", 11, AP_Landing, then_servos_neutral, 0),
125 :
126 : // @Param: ABORT_THR
127 : // @DisplayName: Landing abort using throttle
128 : // @Description: Allow a landing abort to trigger with an input throttle >= 90%. This works with or without stick-mixing enabled.
129 : // @Values: 0:Disabled, 1:Enabled
130 : // @User: Advanced
131 : AP_GROUPINFO("ABORT_THR", 12, AP_Landing, abort_throttle_enable, 0),
132 :
133 : // @Param: FLAP_PERCNT
134 : // @DisplayName: Landing flap percentage
135 : // @Description: The amount of flaps (as a percentage) to apply in the landing approach and flare of an automatic landing
136 : // @Range: 0 100
137 : // @Units: %
138 : // @Increment: 1
139 : // @User: Advanced
140 : AP_GROUPINFO("FLAP_PERCNT", 13, AP_Landing, flap_percent, 0),
141 :
142 : // @Param: OPTIONS
143 : // @DisplayName: Landing options bitmask
144 : // @Description: Bitmask of options to use with landing.
145 : // @Bitmask: 0: honor min throttle during landing flare,1: Increase Target landing airspeed constraint From Trim Airspeed to AIRSPEED_MAX
146 : // @User: Advanced
147 : AP_GROUPINFO("OPTIONS", 16, AP_Landing, _options, 0),
148 :
149 : // @Param: FLARE_AIM
150 : // @DisplayName: Flare aim point adjustment percentage.
151 : // @Description: This parameter controls how much the aim point is moved to allow for the time spent in the flare manoeuvre. When set to 100% the aim point is adjusted on the assumption that the flare sink rate controller instantly achieves the sink rate set by TECS_LAND_SINK. when set to 0%, no aim point adjustment is made. If the plane consistently touches down short of the aim point reduce the parameter and vice verse.
152 : // @Range: 0 100
153 : // @Units: %
154 : // @Increment: 1
155 : // @User: Advanced
156 : AP_GROUPINFO("FLARE_AIM", 17, AP_Landing, flare_effectivness_pct, 50),
157 :
158 : // @Param: WIND_COMP
159 : // @DisplayName: Headwind Compensation when Landing
160 : // @Description: This param controls how much headwind compensation is used when landing. Headwind speed component multiplied by this parameter is added to TECS_LAND_ARSPD command. Set to Zero to disable. Note: The target landing airspeed command is still limited to AIRSPEED_MAX.
161 : // @Range: 0 100
162 : // @Units: %
163 : // @Increment: 1
164 : // @User: Advanced
165 : AP_GROUPINFO("WIND_COMP", 18, AP_Landing, wind_comp, 50),
166 :
167 : // @Param: TYPE
168 : // @DisplayName: Auto-landing type
169 : // @Description: Specifies the auto-landing type to use
170 : // @Values: 0:Standard Glide Slope, 1:Deepstall
171 : // @User: Standard
172 : AP_GROUPINFO_FLAGS("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE, AP_PARAM_FLAG_ENABLE),
173 :
174 : #if HAL_LANDING_DEEPSTALL_ENABLED
175 : // @Group: DS_
176 : // @Path: AP_Landing_Deepstall.cpp
177 : AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),
178 : #endif
179 :
180 : // additional global params should be placed in the list above TYPE to avoid the enable flag hiding the deepstall params
181 :
182 : AP_GROUPEND
183 : };
184 :
185 : // constructor
186 692 : AP_Landing::AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_TECS *_tecs_Controller, AP_Navigation *_nav_controller, AP_FixedWing &_aparm,
187 : set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn,
188 : constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn,
189 : adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn,
190 : adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn,
191 : disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn,
192 692 : update_flight_stage_fn_t _update_flight_stage_fn) :
193 692 : mission(_mission)
194 692 : ,ahrs(_ahrs)
195 692 : ,tecs_Controller(_tecs_Controller)
196 692 : ,nav_controller(_nav_controller)
197 692 : ,aparm(_aparm)
198 692 : ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn)
199 692 : ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn)
200 692 : ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn)
201 692 : ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn)
202 692 : ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn)
203 692 : ,update_flight_stage_fn(_update_flight_stage_fn)
204 : #if HAL_LANDING_DEEPSTALL_ENABLED
205 692 : ,deepstall(*this)
206 : #endif
207 : {
208 692 : AP_Param::setup_object_defaults(this, var_info);
209 692 : }
210 :
211 : /*
212 : return a location alt in cm as AMSL
213 : assumes loc frame is either AMSL or ABOVE_TERRAIN
214 : */
215 24153 : int32_t AP_Landing::loc_alt_AMSL_cm(const Location &loc) const
216 : {
217 : int32_t alt_cm;
218 : // try first with full conversion
219 24153 : if (loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
220 24153 : return alt_cm;
221 : }
222 0 : if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
223 : // if we can't get true terrain then assume flat terrain
224 : // around home
225 0 : return loc.alt + ahrs.get_home().alt;
226 : }
227 :
228 : // this should not happen, but return a value
229 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
230 0 : return loc.alt;
231 : }
232 :
233 114 : void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
234 : {
235 114 : Log(); // log old state so we get a nice transition from old to new here
236 :
237 114 : flags.commanded_go_around = false;
238 :
239 114 : switch (type) {
240 113 : case TYPE_STANDARD_GLIDE_SLOPE:
241 113 : type_slope_do_land(cmd, relative_altitude);
242 113 : break;
243 : #if HAL_LANDING_DEEPSTALL_ENABLED
244 1 : case TYPE_DEEPSTALL:
245 1 : deepstall.do_land(cmd, relative_altitude);
246 1 : break;
247 : #endif
248 0 : default:
249 : // a incorrect type is handled in the verify_land
250 0 : break;
251 : }
252 :
253 114 : Log();
254 114 : }
255 :
256 : /*
257 : update navigation for landing. Called when on landing approach or
258 : final flare
259 : */
260 38886 : bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
261 : 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)
262 : {
263 38886 : bool success = true;
264 :
265 38886 : switch (type) {
266 37991 : case TYPE_STANDARD_GLIDE_SLOPE:
267 37991 : success = type_slope_verify_land(prev_WP_loc, next_WP_loc, current_loc,
268 : height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
269 37991 : break;
270 : #if HAL_LANDING_DEEPSTALL_ENABLED
271 895 : case TYPE_DEEPSTALL:
272 895 : success = deepstall.verify_land(prev_WP_loc, next_WP_loc, current_loc,
273 : height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
274 895 : break;
275 : #endif
276 0 : default:
277 : // returning TRUE while executing verify_land() will increment the
278 : // mission index which in many cases will trigger an RTL for end-of-mission
279 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
280 0 : success = true;
281 0 : break;
282 : }
283 38886 : Log();
284 38886 : return success;
285 : }
286 :
287 247 : bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
288 : const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed)
289 : {
290 247 : switch (type) {
291 247 : case TYPE_STANDARD_GLIDE_SLOPE:
292 247 : type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
293 247 : break;
294 : #if HAL_LANDING_DEEPSTALL_ENABLED
295 0 : case TYPE_DEEPSTALL:
296 0 : deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
297 0 : break;
298 : #endif
299 0 : default:
300 0 : break;
301 : }
302 :
303 : // see if we have reached abort altitude
304 247 : if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
305 2 : next_WP_loc = current_loc;
306 2 : mission.stop();
307 2 : if (restart_landing_sequence()) {
308 2 : mission.resume();
309 : }
310 : // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
311 : #if AP_FENCE_ENABLED
312 2 : AC_Fence *fence = AP::fence();
313 2 : if (fence) {
314 2 : fence->auto_enable_fence_after_takeoff();
315 : }
316 : #endif
317 : }
318 :
319 247 : Log();
320 :
321 : // make sure to always return false so it leaves the mission index alone
322 247 : return false;
323 : }
324 :
325 6253 : void AP_Landing::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)
326 : {
327 6253 : switch (type) {
328 6097 : case TYPE_STANDARD_GLIDE_SLOPE:
329 6097 : type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
330 6097 : break;
331 : #if HAL_LANDING_DEEPSTALL_ENABLED
332 156 : case TYPE_DEEPSTALL:
333 : #endif
334 : default:
335 156 : break;
336 : }
337 6253 : }
338 :
339 : // send out any required mavlink messages
340 552407 : bool AP_Landing::send_landing_message(mavlink_channel_t chan) {
341 552407 : if (!flags.in_progress) {
342 516325 : return false;
343 : }
344 :
345 36082 : switch (type) {
346 : #if HAL_LANDING_DEEPSTALL_ENABLED
347 894 : case TYPE_DEEPSTALL:
348 894 : return deepstall.send_deepstall_message(chan);
349 : #endif
350 35188 : case TYPE_STANDARD_GLIDE_SLOPE:
351 : default:
352 35188 : return false;
353 : }
354 : }
355 :
356 21786200 : bool AP_Landing::is_flaring(void) const
357 : {
358 21786200 : if (!flags.in_progress) {
359 20910225 : return false;
360 : }
361 :
362 875975 : switch (type) {
363 858102 : case TYPE_STANDARD_GLIDE_SLOPE:
364 858102 : return type_slope_is_flaring();
365 : #if HAL_LANDING_DEEPSTALL_ENABLED
366 17873 : case TYPE_DEEPSTALL:
367 : #endif
368 : default:
369 17873 : return false;
370 : }
371 : }
372 :
373 : // return true if the landing is at the pre-flare stage or later
374 1657292 : bool AP_Landing::is_on_final(void) const
375 : {
376 1657292 : if (!flags.in_progress) {
377 1476766 : return false;
378 : }
379 :
380 180526 : switch (type) {
381 176056 : case TYPE_STANDARD_GLIDE_SLOPE:
382 176056 : return type_slope_is_on_final();
383 : #if HAL_LANDING_DEEPSTALL_ENABLED
384 4470 : case TYPE_DEEPSTALL:
385 : #endif
386 : default:
387 4470 : return false;
388 : }
389 : }
390 :
391 : // return true while the aircraft is performing a landing approach
392 : // when true the vehicle will:
393 : // - disable ground steering
394 : // - call setup_landing_glide_slope() and adjust_landing_slope_for_rangefinder_bump()
395 : // - will be considered flying if sink rate > 0.2, and can trigger crash detection
396 1720549 : bool AP_Landing::is_on_approach(void) const
397 : {
398 1720549 : if (!flags.in_progress) {
399 1572040 : return false;
400 : }
401 :
402 148509 : switch (type) {
403 143697 : case TYPE_STANDARD_GLIDE_SLOPE:
404 143697 : return type_slope_is_on_approach();
405 : #if HAL_LANDING_DEEPSTALL_ENABLED
406 4812 : case TYPE_DEEPSTALL:
407 4812 : return deepstall.is_on_approach();
408 : #endif
409 0 : default:
410 0 : return false;
411 : }
412 : }
413 :
414 : // return true while the aircraft is allowed to perform ground steering
415 6174463 : bool AP_Landing::is_ground_steering_allowed(void) const
416 : {
417 6174463 : if (!flags.in_progress) {
418 6099912 : return true;
419 : }
420 :
421 74551 : switch (type) {
422 70081 : case TYPE_STANDARD_GLIDE_SLOPE:
423 70081 : return type_slope_is_on_approach();
424 : #if HAL_LANDING_DEEPSTALL_ENABLED
425 4470 : case TYPE_DEEPSTALL:
426 4470 : return false;
427 : #endif
428 0 : default:
429 0 : return true;
430 : }
431 : }
432 :
433 : // return true when at the last stages of a land when an impact with the ground is expected soon
434 : // when true is_flying knows that the vehicle was expecting to stop flying, possibly because of a hard impact
435 5688 : bool AP_Landing::is_expecting_impact(void) const
436 : {
437 5688 : if (!flags.in_progress) {
438 4998 : return false;
439 : }
440 :
441 690 : switch (type) {
442 690 : case TYPE_STANDARD_GLIDE_SLOPE:
443 690 : return type_slope_is_expecting_impact();
444 : #if HAL_LANDING_DEEPSTALL_ENABLED
445 0 : case TYPE_DEEPSTALL:
446 : #endif
447 : default:
448 0 : return false;
449 : }
450 : }
451 :
452 : // returns true when the landing library has overriden any servos
453 197641 : bool AP_Landing::override_servos(void) {
454 197641 : if (!flags.in_progress) {
455 0 : return false;
456 : }
457 :
458 197641 : switch (type) {
459 : #if HAL_LANDING_DEEPSTALL_ENABLED
460 4470 : case TYPE_DEEPSTALL:
461 4470 : return deepstall.override_servos();
462 : #endif
463 193171 : case TYPE_STANDARD_GLIDE_SLOPE:
464 : default:
465 193171 : return false;
466 : }
467 : }
468 :
469 : // returns a PID_Info object if there is one available for the selected landing
470 : // type, otherwise returns a nullptr, indicating no data to be logged/sent
471 0 : const AP_PIDInfo* AP_Landing::get_pid_info(void) const
472 : {
473 0 : switch (type) {
474 : #if HAL_LANDING_DEEPSTALL_ENABLED
475 0 : case TYPE_DEEPSTALL:
476 0 : return &deepstall.get_pid_info();
477 : #endif
478 0 : case TYPE_STANDARD_GLIDE_SLOPE:
479 : default:
480 0 : return nullptr;
481 : }
482 : }
483 :
484 : /*
485 : a special glide slope calculation for the landing approach
486 :
487 : During the land approach use a linear glide slope to a point
488 : projected through the landing point. We don't use the landing point
489 : itself as that leads to discontinuities close to the landing point,
490 : which can lead to erratic pitch control
491 : */
492 :
493 6253 : void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location ¤t_loc, int32_t &target_altitude_offset_cm)
494 : {
495 6253 : switch (type) {
496 6097 : case TYPE_STANDARD_GLIDE_SLOPE:
497 6097 : type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
498 6097 : break;
499 : #if HAL_LANDING_DEEPSTALL_ENABLED
500 156 : case TYPE_DEEPSTALL:
501 : #endif
502 : default:
503 156 : break;
504 : }
505 6253 : }
506 :
507 : /*
508 : reset landing state
509 : */
510 1929 : void AP_Landing::reset(void)
511 : {
512 1929 : initial_slope = 0;
513 1929 : slope = 0;
514 1929 : type_slope_flags.post_stats = false;
515 1929 : type_slope_flags.has_aborted_due_to_slope_recalc = false;
516 1929 : type_slope_stage = SlopeStage::NORMAL;
517 1929 : }
518 :
519 : /*
520 : Restart a landing by first checking for a DO_LAND_START and
521 : jump there. Otherwise decrement waypoint so we would re-start
522 : from the top with same glide slope. Return true if successful.
523 : */
524 58 : bool AP_Landing::restart_landing_sequence()
525 : {
526 58 : if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
527 0 : return false;
528 : }
529 :
530 58 : uint16_t do_land_start_index = 0;
531 58 : Location loc;
532 58 : if (ahrs.get_location(loc)) {
533 58 : do_land_start_index = mission.get_landing_sequence_start(loc);
534 : }
535 58 : uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
536 58 : bool success = false;
537 58 : uint16_t current_index = mission.get_current_nav_index();
538 58 : AP_Mission::Mission_Command cmd;
539 :
540 58 : if (mission.read_cmd_from_storage(current_index+1,cmd) &&
541 3 : cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT &&
542 61 : (cmd.p1 == 0 || cmd.p1 == 1) &&
543 0 : mission.set_current_cmd(current_index+1))
544 : {
545 : // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
546 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", (signed)cmd.content.location.alt/100);
547 0 : success = true;
548 : }
549 64 : else if (do_land_start_index != 0 &&
550 6 : mission.set_current_cmd(do_land_start_index))
551 : {
552 : // look for a DO_LAND_START and use that index
553 6 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
554 6 : success = true;
555 : }
556 104 : else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
557 52 : mission.set_current_cmd(prev_cmd_with_wp_index))
558 : {
559 : // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
560 : // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
561 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
562 0 : success = true;
563 : } else {
564 52 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
565 52 : success = false;
566 : }
567 :
568 58 : if (success) {
569 : // exit landing stages if we're no longer executing NAV_LAND
570 6 : update_flight_stage_fn();
571 : }
572 :
573 58 : Log();
574 58 : return success;
575 : }
576 :
577 207583 : int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
578 : {
579 207583 : switch (type) {
580 203106 : case TYPE_STANDARD_GLIDE_SLOPE:
581 203106 : return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd);
582 : #if HAL_LANDING_DEEPSTALL_ENABLED
583 4477 : case TYPE_DEEPSTALL:
584 : #endif
585 : default:
586 4477 : return desired_roll_cd;
587 : }
588 : }
589 :
590 : // returns true if landing provided a Location structure with the current target altitude
591 497799 : bool AP_Landing::get_target_altitude_location(Location &location)
592 : {
593 497799 : if (!flags.in_progress) {
594 491092 : return false;
595 : }
596 :
597 6707 : switch (type) {
598 : #if HAL_LANDING_DEEPSTALL_ENABLED
599 739 : case TYPE_DEEPSTALL:
600 739 : return deepstall.get_target_altitude_location(location);
601 : #endif
602 5968 : case TYPE_STANDARD_GLIDE_SLOPE:
603 : default:
604 5968 : return false;
605 : }
606 : }
607 :
608 : /*
609 : * returns target airspeed in m/s depending on flight stage
610 : */
611 36337 : float AP_Landing::get_target_airspeed_ms(void)
612 : {
613 36337 : if (!flags.in_progress) {
614 : // not landing, use regular cruise airspeed
615 0 : return aparm.airspeed_cruise;
616 : }
617 :
618 36337 : switch (type) {
619 35442 : case TYPE_STANDARD_GLIDE_SLOPE:
620 35442 : return type_slope_get_target_airspeed_ms();
621 : #if HAL_LANDING_DEEPSTALL_ENABLED
622 895 : case TYPE_DEEPSTALL:
623 895 : return deepstall.get_target_airspeed_ms();
624 : #endif
625 0 : default:
626 : // don't return the landing airspeed, because if type is invalid we have
627 : // no postive indication that the land airspeed has been configured or
628 : // how it was meant to be utilized
629 0 : return tecs_Controller->get_target_airspeed();
630 : }
631 : }
632 :
633 : /*
634 : * request a landing abort given the landing type
635 : * return true on success
636 : */
637 2 : bool AP_Landing::request_go_around(void)
638 : {
639 2 : bool success = false;
640 :
641 2 : switch (type) {
642 2 : case TYPE_STANDARD_GLIDE_SLOPE:
643 2 : success = type_slope_request_go_around();
644 2 : break;
645 : #if HAL_LANDING_DEEPSTALL_ENABLED
646 0 : case TYPE_DEEPSTALL:
647 0 : success = deepstall.request_go_around();
648 0 : break;
649 : #endif
650 0 : default:
651 0 : break;
652 : }
653 :
654 2 : Log();
655 2 : return success;
656 : }
657 :
658 8085 : void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage)
659 : {
660 8085 : Log(); // log old value to plot discrete transitions
661 8085 : flags.in_progress = _in_landing_stage;
662 8085 : flags.commanded_go_around = false;
663 8085 : Log();
664 8085 : }
665 :
666 : /*
667 : * returns true when a landing is complete, usually used to disable throttle
668 : */
669 0 : bool AP_Landing::is_complete(void) const
670 : {
671 0 : switch (type) {
672 0 : case TYPE_STANDARD_GLIDE_SLOPE:
673 0 : return type_slope_is_complete();
674 : #if HAL_LANDING_DEEPSTALL_ENABLED
675 0 : case TYPE_DEEPSTALL:
676 0 : return false;
677 : #endif
678 0 : default:
679 0 : return true;
680 : }
681 : }
682 :
683 55591 : void AP_Landing::Log(void) const
684 : {
685 : #if HAL_LOGGING_ENABLED
686 55591 : switch (type) {
687 54686 : case TYPE_STANDARD_GLIDE_SLOPE:
688 54686 : type_slope_log();
689 54686 : break;
690 : #if HAL_LANDING_DEEPSTALL_ENABLED
691 905 : case TYPE_DEEPSTALL:
692 905 : deepstall.Log();
693 905 : break;
694 : #endif
695 0 : default:
696 0 : break;
697 : }
698 : #endif
699 55591 : }
700 :
701 : /*
702 : * returns true when throttle should be suppressed while landing
703 : */
704 7998449 : bool AP_Landing::is_throttle_suppressed(void) const
705 : {
706 7998449 : if (!flags.in_progress) {
707 7603167 : return false;
708 : }
709 :
710 395282 : switch (type) {
711 386342 : case TYPE_STANDARD_GLIDE_SLOPE:
712 386342 : return type_slope_is_throttle_suppressed();
713 : #if HAL_LANDING_DEEPSTALL_ENABLED
714 8940 : case TYPE_DEEPSTALL:
715 8940 : return deepstall.is_throttle_suppressed();
716 : #endif
717 0 : default:
718 0 : return false;
719 : }
720 : }
721 :
722 : //defaults to false, but _options bit zero enables it.
723 123090 : bool AP_Landing::use_thr_min_during_flare(void) const
724 : {
725 123090 : return (OptionsMask::ON_LANDING_FLARE_USE_THR_MIN & _options) != 0;
726 : }
727 :
728 : //defaults to false, but _options bit zero enables it.
729 35442 : bool AP_Landing::allow_max_airspeed_on_land(void) const
730 : {
731 35442 : return (OptionsMask::ON_LANDING_USE_ARSPD_MAX & _options) != 0;
732 : }
733 :
734 : /*
735 : * returns false when the vehicle might not be flying forward while landing
736 : */
737 197641 : bool AP_Landing::is_flying_forward(void) const
738 : {
739 197641 : if (!flags.in_progress) {
740 0 : return true;
741 : }
742 :
743 197641 : switch (type) {
744 : #if HAL_LANDING_DEEPSTALL_ENABLED
745 4470 : case TYPE_DEEPSTALL:
746 4470 : return deepstall.is_flying_forward();
747 : #endif
748 193171 : case TYPE_STANDARD_GLIDE_SLOPE:
749 : default:
750 193171 : return true;
751 : }
752 : }
753 :
754 : /*
755 : * attempt to terminate flight with an immediate landing
756 : * returns true if the landing library can and is terminating the landing
757 : */
758 0 : bool AP_Landing::terminate(void) {
759 0 : switch (type) {
760 : #if HAL_LANDING_DEEPSTALL_ENABLED
761 0 : case TYPE_DEEPSTALL:
762 0 : return deepstall.terminate();
763 : #endif
764 0 : case TYPE_STANDARD_GLIDE_SLOPE:
765 : default:
766 0 : return false;
767 : }
768 : }
769 :
770 : /*
771 : run parameter conversions
772 : */
773 692 : void AP_Landing::convert_parameters(void)
774 : {
775 : // PARAMETER_CONVERSION - Added: Jan-2024 for ArduPilot-4.5
776 692 : pitch_deg.convert_centi_parameter(AP_PARAM_INT16);
777 :
778 : // PARAMETER_CONVERSION - Added: Mar-2026 for THR_SLEWRATE width change
779 : // Convert throttle slewrate from int8 to int16 to support higher slew rates
780 692 : throttle_slewrate.convert_parameter_width(AP_PARAM_INT8);
781 692 : }
|
