APM:Libraries
AP_Landing.cpp
Go to the documentation of this file.
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 
23 // table of user settable parameters
25 
26  // @Param: SLOPE_RCALC
27  // @DisplayName: Landing slope re-calc threshold
28  // @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.
29  // @Range: 0 5
30  // @Units: m
31  // @Increment: 0.5
32  // @User: Advanced
33  AP_GROUPINFO("SLOPE_RCALC", 1, AP_Landing, slope_recalc_shallow_threshold, 2.0f),
34 
35  // @Param: ABORT_DEG
36  // @DisplayName: Landing auto-abort slope threshold
37  // @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.
38  // @Range: 0 90
39  // @Units: deg
40  // @Increment: 0.1
41  // @User: Advanced
42  AP_GROUPINFO("ABORT_DEG", 2, AP_Landing, slope_recalc_steep_threshold_to_abort, 0),
43 
44  // @Param: PITCH_CD
45  // @DisplayName: Landing Pitch
46  // @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.
47  // @Units: cdeg
48  // @User: Advanced
49  AP_GROUPINFO("PITCH_CD", 3, AP_Landing, pitch_cd, 0),
50 
51  // @Param: FLARE_ALT
52  // @DisplayName: Landing flare altitude
53  // @Description: Altitude in autoland at which to lock heading and flare to the LAND_PITCH_CD 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.
54  // @Units: m
55  // @Increment: 0.1
56  // @User: Advanced
57  AP_GROUPINFO("FLARE_ALT", 4, AP_Landing, flare_alt, 3.0f),
58 
59  // @Param: FLARE_SEC
60  // @DisplayName: Landing flare time
61  // @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).
62  // @Units: s
63  // @Increment: 0.1
64  // @User: Advanced
65  AP_GROUPINFO("FLARE_SEC", 5, AP_Landing, flare_sec, 2.0f),
66 
67  // @Param: PF_ALT
68  // @DisplayName: Landing pre-flare altitude
69  // @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.
70  // @Units: m
71  // @Range: 0 30
72  // @Increment: 0.1
73  // @User: Advanced
74  AP_GROUPINFO("PF_ALT", 6, AP_Landing, pre_flare_alt, 10.0f),
75 
76  // @Param: PF_SEC
77  // @DisplayName: Landing pre-flare time
78  // @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.
79  // @Units: s
80  // @Range: 0 10
81  // @Increment: 0.1
82  // @User: Advanced
83  AP_GROUPINFO("PF_SEC", 7, AP_Landing, pre_flare_sec, 6.0f),
84 
85  // @Param: PF_ARSPD
86  // @DisplayName: Landing pre-flare airspeed
87  // @Description: Desired airspeed during pre-flare flight stage. This is useful to reduce airspeed just before the flare. Use 0 to disable.
88  // @Units: m/s
89  // @Range: 0 30
90  // @Increment: 0.1
91  // @User: Advanced
92  AP_GROUPINFO("PF_ARSPD", 8, AP_Landing, pre_flare_airspeed, 0),
93 
94  // @Param: THR_SLEW
95  // @DisplayName: Landing throttle slew rate
96  // @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.
97  // @Units: %
98  // @Range: 0 127
99  // @Increment: 1
100  // @User: User
101  AP_GROUPINFO("THR_SLEW", 9, AP_Landing, throttle_slewrate, 0),
102 
103  // @Param: DISARMDELAY
104  // @DisplayName: Landing disarm delay
105  // @Description: After a landing has completed using a LAND waypoint, automatically disarm after this many seconds have passed. Use 0 to not disarm.
106  // @Units: s
107  // @Increment: 1
108  // @Range: 0 127
109  // @User: Advanced
110  AP_GROUPINFO("DISARMDELAY", 10, AP_Landing, disarm_delay, 20),
111 
112  // @Param: THEN_NEUTRL
113  // @DisplayName: Set servos to neutral after landing
114  // @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.
115  // @Values: 0:Disabled, 1:Servos to Neutral, 2:Servos to Zero PWM
116  // @User: Advanced
117  AP_GROUPINFO("THEN_NEUTRL", 11, AP_Landing, then_servos_neutral, 0),
118 
119  // @Param: ABORT_THR
120  // @DisplayName: Landing abort using throttle
121  // @Description: Allow a landing abort to trigger with a throttle > 95%
122  // @Values: 0:Disabled, 1:Enabled
123  // @User: Advanced
124  AP_GROUPINFO("ABORT_THR", 12, AP_Landing, abort_throttle_enable, 0),
125 
126  // @Param: FLAP_PERCNT
127  // @DisplayName: Landing flap percentage
128  // @Description: The amount of flaps (as a percentage) to apply in the landing approach and flare of an automatic landing
129  // @Range: 0 100
130  // @Units: %
131  // @User: Advanced
132  AP_GROUPINFO("FLAP_PERCNT", 13, AP_Landing, flap_percent, 0),
133 
134  // @Param: TYPE
135  // @DisplayName: Auto-landing type
136  // @Description: Specifies the auto-landing type to use
137  // @Values: 0:Standard Glide Slope, 1:Deepstall
138  // @User: Standard
139  AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE),
140 
141  // @Group: DS_
142  // @Path: AP_Landing_Deepstall.cpp
143  AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),
144 
146 };
147 
148  // constructor
149 AP_Landing::AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm,
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  mission(_mission)
157  ,ahrs(_ahrs)
158  ,SpdHgt_Controller(_SpdHgt_Controller)
159  ,nav_controller(_nav_controller)
160  ,aparm(_aparm)
161  ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn)
162  ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn)
163  ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn)
164  ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn)
165  ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn)
166  ,update_flight_stage_fn(_update_flight_stage_fn)
167  ,deepstall(*this)
168 {
170 }
171 
172 
173 void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
174 {
175  Log(); // log old state so we get a nice transition from old to new here
176 
177  flags.commanded_go_around = false;
178 
179  switch (type) {
181  type_slope_do_land(cmd, relative_altitude);
182  break;
183  case TYPE_DEEPSTALL:
184  deepstall.do_land(cmd, relative_altitude);
185  break;
186  default:
187  // a incorrect type is handled in the verify_land
188  break;
189  }
190 
191  Log();
192 }
193 
194 /*
195  update navigation for landing. Called when on landing approach or
196  final flare
197  */
198 bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
199  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)
200 {
201  bool success = true;
202 
203  switch (type) {
205  success = type_slope_verify_land(prev_WP_loc, next_WP_loc, current_loc,
206  height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
207  break;
208  case TYPE_DEEPSTALL:
209  success = deepstall.verify_land(prev_WP_loc, next_WP_loc, current_loc,
210  height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
211  break;
212  default:
213  // returning TRUE while executing verify_land() will increment the
214  // mission index which in many cases will trigger an RTL for end-of-mission
215  gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
216  success = true;
217  break;
218  }
219  Log();
220  return success;
221 }
222 
223 
224 bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
225  const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed)
226 {
227  switch (type) {
229  type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
230  break;
231  case TYPE_DEEPSTALL:
232  deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
233  break;
234  default:
235  break;
236  }
237 
238  // see if we have reached abort altitude
239  if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
240  next_WP_loc = current_loc;
241  mission.stop();
242  if (restart_landing_sequence()) {
243  mission.resume();
244  }
245  // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
246  }
247 
248  Log();
249 
250  // make sure to always return false so it leaves the mission index alone
251  return false;
252 }
253 
254 void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm)
255 {
256  switch (type) {
258  type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
259  break;
260  case TYPE_DEEPSTALL:
261  default:
262  break;
263  }
264 }
265 
266 // send out any required mavlink messages
267 bool AP_Landing::send_landing_message(mavlink_channel_t chan) {
268  if (!flags.in_progress) {
269  return false;
270  }
271 
272  switch (type) {
273  case TYPE_DEEPSTALL:
274  return deepstall.send_deepstall_message(chan);
276  default:
277  return false;
278  }
279 }
280 
281 bool AP_Landing::is_flaring(void) const
282 {
283  if (!flags.in_progress) {
284  return false;
285  }
286 
287  switch (type) {
289  return type_slope_is_flaring();
290  case TYPE_DEEPSTALL:
291  default:
292  return false;
293  }
294 }
295 
296 // return true while the aircraft is performing a landing approach
297 // when true the vehicle will:
298 // - disable ground steering
299 // - call setup_landing_glide_slope() and adjust_landing_slope_for_rangefinder_bump()
300 // - will be considered flying if sink rate > 0.2, and can trigger crash detection
302 {
303  if (!flags.in_progress) {
304  return false;
305  }
306 
307  switch (type) {
309  return type_slope_is_on_approach();
310  case TYPE_DEEPSTALL:
311  return deepstall.is_on_approach();
312  default:
313  return false;
314  }
315 }
316 
317 // return true while the aircraft is allowed to perform ground steering
319 {
320  if (!flags.in_progress) {
321  return true;
322  }
323 
324  switch (type) {
326  return type_slope_is_on_approach();
327  case TYPE_DEEPSTALL:
328  return false;
329  default:
330  return true;
331  }
332 }
333 
334 // return true when at the last stages of a land when an impact with the ground is expected soon
335 // when true is_flying knows that the vehicle was expecting to stop flying, possibly because of a hard impact
337 {
338  if (!flags.in_progress) {
339  return false;
340  }
341 
342  switch (type) {
345  case TYPE_DEEPSTALL:
346  default:
347  return false;
348  }
349 }
350 
351 // returns true when the landing library has overriden any servos
353  if (!flags.in_progress) {
354  return false;
355  }
356 
357  switch (type) {
358  case TYPE_DEEPSTALL:
359  return deepstall.override_servos();
361  default:
362  return false;
363  }
364 }
365 
366 // returns a PID_Info object if there is one available for the selected landing
367 // type, otherwise returns a nullptr, indicating no data to be logged/sent
369 {
370  switch (type) {
371  case TYPE_DEEPSTALL:
372  return &deepstall.get_pid_info();
374  default:
375  return nullptr;
376  }
377 }
378 
379 /*
380  a special glide slope calculation for the landing approach
381 
382  During the land approach use a linear glide slope to a point
383  projected through the landing point. We don't use the landing point
384  itself as that leads to discontinuities close to the landing point,
385  which can lead to erratic pitch control
386  */
387 
388 void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
389 {
390  switch (type) {
392  type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
393  break;
394  case TYPE_DEEPSTALL:
395  default:
396  break;
397  }
398 }
399 
400 /*
401  Restart a landing by first checking for a DO_LAND_START and
402  jump there. Otherwise decrement waypoint so we would re-start
403  from the top with same glide slope. Return true if successful.
404  */
406 {
407  if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
408  return false;
409  }
410 
411  uint16_t do_land_start_index = mission.get_landing_sequence_start();
412  uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
413  bool success = false;
414  uint16_t current_index = mission.get_current_nav_index();
416 
417  if (mission.read_cmd_from_storage(current_index+1,cmd) &&
418  cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT &&
419  (cmd.p1 == 0 || cmd.p1 == 1) &&
420  mission.set_current_cmd(current_index+1))
421  {
422  // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
423  gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100);
424  success = true;
425  }
426  else if (do_land_start_index != 0 &&
427  mission.set_current_cmd(do_land_start_index))
428  {
429  // look for a DO_LAND_START and use that index
430  gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
431  success = true;
432  }
433  else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
434  mission.set_current_cmd(prev_cmd_with_wp_index))
435  {
436  // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
437  // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
438  gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
439  success = true;
440  } else {
441  gcs().send_text(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
442  success = false;
443  }
444 
445  if (success) {
446  // exit landing stages if we're no longer executing NAV_LAND
448  }
449 
450  Log();
451  return success;
452 }
453 
454 int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
455 {
456  switch (type) {
458  return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd);
459  case TYPE_DEEPSTALL:
460  default:
461  return desired_roll_cd;
462  }
463 }
464 
465 // returns true if landing provided a Location structure with the current target altitude
467 {
468  if (!flags.in_progress) {
469  return false;
470  }
471 
472  switch (type) {
473  case TYPE_DEEPSTALL:
474  return deepstall.get_target_altitude_location(location);
476  default:
477  return false;
478  }
479 }
480 
481 /*
482  * Determine how aligned heading_deg is with the wind. Return result
483  * is 1.0 when perfectly aligned heading into wind, -1 when perfectly
484  * aligned with-wind, and zero when perfect cross-wind. There is no
485  * distinction between a left or right cross-wind. Wind speed is ignored
486  */
487 float AP_Landing::wind_alignment(const float heading_deg)
488 {
489  const Vector3f wind = ahrs.wind_estimate();
490  const float wind_heading_rad = atan2f(-wind.y, -wind.x);
491  return cosf(wind_heading_rad - radians(heading_deg));
492 }
493 
494 /*
495  * returns head-wind in m/s, 0 for tail-wind.
496  */
498 {
499  const float alignment = wind_alignment(ahrs.yaw_sensor*0.01f);
500 
501  if (alignment <= 0) {
502  return 0;
503  }
504 
505  return alignment * ahrs.wind_estimate().length();
506 }
507 
508 /*
509  * returns target airspeed in cm/s depending on flight stage
510  */
512 {
513  if (!flags.in_progress) {
514  // not landing, use regular cruise airspeed
515  return aparm.airspeed_cruise_cm;
516  }
517 
518  switch (type) {
521  case TYPE_DEEPSTALL:
523  default:
524  // don't return the landing airspeed, because if type is invalid we have
525  // no postive indication that the land airspeed has been configured or
526  // how it was meant to be utilized
528  }
529 }
530 
531 /*
532  * request a landing abort given the landing type
533  * return true on success
534  */
536 {
537  bool success = false;
538 
539  switch (type) {
541  success = type_slope_request_go_around();
542  break;
543  case TYPE_DEEPSTALL:
544  success = deepstall.request_go_around();
545  break;
546  default:
547  break;
548  }
549 
550  Log();
551  return success;
552 }
553 
554 void AP_Landing::handle_flight_stage_change(const bool _in_landing_stage)
555 {
556  Log(); // log old value to plot discrete transitions
557  flags.in_progress = _in_landing_stage;
558  flags.commanded_go_around = false;
559  Log();
560 }
561 
562 /*
563  * returns true when a landing is complete, usually used to disable throttle
564  */
565 bool AP_Landing::is_complete(void) const
566 {
567  switch (type) {
569  return type_slope_is_complete();
570  case TYPE_DEEPSTALL:
571  return false;
572  default:
573  return true;
574  }
575 }
576 
577 void AP_Landing::Log(void) const
578 {
579  switch (type) {
581  type_slope_log();
582  break;
583  case TYPE_DEEPSTALL:
584  deepstall.Log();
585  break;
586  default:
587  break;
588  }
589 }
590 
591 /*
592  * returns true when throttle should be suppressed while landing
593  */
595 {
596  if (!flags.in_progress) {
597  return false;
598  }
599 
600  switch (type) {
603  case TYPE_DEEPSTALL:
605  default:
606  return false;
607  }
608 }
609 
610 /*
611  * returns false when the vehicle might not be flying forward while landing
612  */
614 {
615  if (!flags.in_progress) {
616  return true;
617  }
618 
619  switch (type) {
620  case TYPE_DEEPSTALL:
621  return deepstall.is_flying_forward();
623  default:
624  return true;
625  }
626 }
627 
628 /*
629  * attempt to terminate flight with an immediate landing
630  * returns true if the landing library can and is terminating the landing
631  */
633  switch (type) {
634  case TYPE_DEEPSTALL:
635  return deepstall.terminate();
637  default:
638  return false;
639  }
640 }
int32_t get_target_airspeed_cm(void) const
void type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm)
AP_AHRS & ahrs
Definition: AP_Landing.h:125
bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed)
Definition: AP_Landing.cpp:224
bool is_on_approach(void) const
void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
Definition: AP_Landing.cpp:388
void type_slope_setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
bool send_landing_message(mavlink_channel_t chan)
Definition: AP_Landing.cpp:267
void verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
bool type_slope_is_on_approach(void) const
bool type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, 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)
bool get_target_altitude_location(Location &location)
AP_AHRS_NavEKF & ahrs
Definition: AHRS_Test.cpp:39
float wind_alignment(const float heading_deg)
Definition: AP_Landing.cpp:487
void handle_flight_stage_change(const bool _in_landing_stage)
Definition: AP_Landing.cpp:554
bool type_slope_request_go_around(void)
Interface definition for the various Ground Control System.
Object managing Mission.
Definition: AP_Mission.h:45
uint16_t get_current_nav_index() const
Definition: AP_Mission.h:389
virtual Vector3f wind_estimate(void) const =0
#define AP_GROUPINFO(name, idx, clazz, element, def)
Definition: AP_Param.h:102
GCS & gcs()
bool type_slope_is_complete(void) const
void do_land(const AP_Mission::Mission_Command &cmd, const float relative_altitude)
const Mission_Command & get_current_nav_cmd() const
get_current_nav_cmd - returns the current "navigation" command
Definition: AP_Mission.h:384
AP_Mission & mission
Definition: AP_Landing.h:124
AP_SpdHgtControl * SpdHgt_Controller
Definition: AP_Landing.h:126
bool read_cmd_from_storage(uint16_t index, Mission_Command &cmd) const
Definition: AP_Mission.cpp:453
void stop()
stop - stops mission execution. subsequent calls to update() will have no effect until the mission is...
Definition: AP_Mission.cpp:79
bool get_target_altitude_location(Location &location)
Definition: AP_Landing.cpp:466
bool request_go_around(void)
Definition: AP_Landing.cpp:535
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
Definition: AP_Common.h:142
void Log(void) const
Definition: AP_Landing.cpp:577
#define f(i)
T y
Definition: vector3.h:67
Class managing Plane Deepstall landing methods.
bool send_deepstall_message(mavlink_channel_t chan) const
bool is_ground_steering_allowed(void) const
Definition: AP_Landing.cpp:318
bool is_expecting_impact(void) const
Definition: AP_Landing.cpp:336
uint16_t get_prev_nav_cmd_with_wp_index() const
Definition: AP_Mission.h:405
bool is_flying_forward(void) const
Definition: AP_Landing.cpp:613
struct AP_Landing::@124 flags
AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm, set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn, constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn, adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn, adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn, disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn, update_flight_stage_fn_t _update_flight_stage_fn)
Definition: AP_Landing.cpp:149
bool set_current_cmd(uint16_t index)
Definition: AP_Mission.cpp:339
uint16_t get_landing_sequence_start()
int32_t get_target_airspeed_cm(void)
Definition: AP_Landing.cpp:511
AP_Int8 type
Definition: AP_Landing.h:154
bool override_servos(void)
Definition: AP_Landing.cpp:352
void send_text(MAV_SEVERITY severity, const char *fmt,...)
Definition: GCS.cpp:8
int32_t type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
Class managing ArduPlane landing methods.
Definition: AP_Landing.h:28
void resume()
Definition: AP_Mission.cpp:86
bool type_slope_is_flaring(void) const
update_flight_stage_fn_t update_flight_stage_fn
Definition: AP_Landing.h:136
void type_slope_log(void) const
bool is_throttle_suppressed(void) const
float length(void) const
Definition: vector3.cpp:288
bool type_slope_is_expecting_impact(void) const
Vector2f location
Definition: location.cpp:16
bool is_complete(void) const
Definition: AP_Landing.cpp:565
void do_land(const AP_Mission::Mission_Command &cmd, const float relative_altitude)
Definition: AP_Landing.cpp:173
static constexpr float radians(float deg)
Definition: AP_Math.h:158
bool terminate(void)
Definition: AP_Landing.cpp:632
const DataFlash_Class::PID_Info * get_pid_info(void) const
Definition: AP_Landing.cpp:368
bool is_flaring(void) const
Definition: AP_Landing.cpp:281
bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, 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)
Definition: AP_Landing.cpp:198
#define AP_MISSION_CMD_INDEX_NONE
Definition: AP_Mission.h:33
AP_HAL::AnalogSource * chan
Definition: AnalogIn.cpp:8
AP_Landing_Deepstall deepstall
Definition: AP_Landing.h:139
int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
Definition: AP_Landing.cpp:454
bool restart_landing_sequence(void)
Definition: AP_Landing.cpp:405
bool type_slope_is_throttle_suppressed(void) const
bool is_throttle_suppressed(void) const
Definition: AP_Landing.cpp:594
int32_t type_slope_get_target_airspeed_cm(void)
AP_Vehicle::FixedWing & aparm
Definition: AP_Landing.h:129
float head_wind(void)
Definition: AP_Landing.cpp:497
void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
void type_slope_do_land(const AP_Mission::Mission_Command &cmd, const float relative_altitude)
virtual float get_target_airspeed(void) const =0
static const struct AP_Param::GroupInfo var_info[]
Definition: AP_Landing.h:90
bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, 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)
void adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm)
Definition: AP_Landing.cpp:254
bool is_flying_forward(void) const
#define AP_SUBGROUPINFO(element, name, idx, thisclazz, elclazz)
Definition: AP_Param.h:109
int32_t yaw_sensor
Definition: AP_AHRS.h:231
adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn
Definition: AP_Landing.h:134
#define AP_GROUPEND
Definition: AP_Param.h:121
bool is_on_approach(void) const
Definition: AP_Landing.cpp:301
T x
Definition: vector3.h:67
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
Definition: AP_Param.cpp:1214
AP_Int32 airspeed_cruise_cm
Definition: AP_Vehicle.h:38
const DataFlash_Class::PID_Info & get_pid_info(void) const