AP_Landing.cpp 23 KB

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