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_Slope.cpp - Landing logic handler for ArduPlane for STANDARD_GLIDE_SLOPE
18 : */
19 :
20 : #include "AP_Landing.h"
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <AP_LandingGear/AP_LandingGear.h>
24 : #include <AP_AHRS/AP_AHRS.h>
25 : #include <AP_GPS/AP_GPS.h>
26 : #include <AP_Logger/AP_Logger.h>
27 :
28 : #if defined(APM_BUILD_TYPE)
29 : // - this is just here to encourage the build system to supply the "legacy build defines". The actual dependecy is in the AP_LandingGear.h and AP_LandingGear_config.h headers
30 : #endif
31 :
32 113 : void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
33 : {
34 113 : initial_slope = 0;
35 113 : slope = 0;
36 :
37 : // once landed, post some landing statistics to the GCS
38 113 : type_slope_flags.post_stats = false;
39 :
40 113 : type_slope_stage = SlopeStage::NORMAL;
41 113 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
42 113 : }
43 :
44 247 : void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
45 : {
46 : // when aborting a landing, mimic the verify_takeoff with steering hold. Once
47 : // the altitude has been reached, restart the landing sequence
48 247 : throttle_suppressed = false;
49 247 : nav_controller->update_heading_hold(prev_WP_loc.get_bearing_to(next_WP_loc));
50 247 : }
51 :
52 : /*
53 : update navigation for landing. Called when on landing approach or
54 : final flare
55 : */
56 37991 : bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
57 : 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)
58 : {
59 : // we don't 'verify' landing in the sense that it never completes,
60 : // so we don't verify command completion. Instead we use this to
61 : // adjust final landing parameters
62 :
63 : // determine stage
64 37991 : if (type_slope_stage == SlopeStage::NORMAL) {
65 5862 : const bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
66 5862 : const bool on_flight_line = fabsf(nav_controller->crosstrack_error_m()) < 5.0f && !nav_controller->data_is_stale();
67 5862 : const bool below_prev_WP = current_loc.alt < loc_alt_AMSL_cm(prev_WP_loc);
68 11724 : if ((mission.get_prev_nav_cmd_id() == MAV_CMD_NAV_LOITER_TO_ALT) ||
69 5862 : (wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
70 11724 : (wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
71 : (wp_proportion > 0.5f)) {
72 111 : type_slope_stage = SlopeStage::APPROACH;
73 : }
74 : }
75 :
76 : /* Set land_complete (which starts the flare) under 3 conditions:
77 : 1) we are within LAND_FLARE_ALT meters of the landing altitude
78 : 2) we are within LAND_FLARE_SEC of the landing point vertically
79 : by the calculated sink rate (if LAND_FLARE_SEC != 0)
80 : 3) we have gone past the landing point and don't have
81 : rangefinder data (to prevent us keeping throttle on
82 : after landing if we've had positive baro drift)
83 : */
84 :
85 : // flare check:
86 : // 1) below flare alt/sec requires approach stage check because if sec/alt are set too
87 : // large, and we're on a hard turn to line up for approach, we'll prematurely flare by
88 : // skipping approach phase and the extreme roll limits will make it hard to line up with runway
89 : // 2) passed land point and don't have an accurate AGL
90 : // 3) probably crashed (ensures motor gets turned off)
91 :
92 37991 : const bool on_approach_stage = type_slope_is_on_approach();
93 37991 : const bool below_flare_alt = (height <= flare_alt);
94 37991 : const bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec);
95 37991 : const bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);
96 :
97 37991 : height_flare_log = height;
98 :
99 37991 : const AP_GPS &gps = AP::gps();
100 :
101 37991 : if ((on_approach_stage && below_flare_alt) ||
102 6203 : (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
103 37887 : (!rangefinder_state_in_range && wp_proportion >= 1) ||
104 : probably_crashed) {
105 :
106 18153 : if (type_slope_stage != SlopeStage::FINAL) {
107 110 : type_slope_flags.post_stats = true;
108 110 : if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
109 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
110 : } else {
111 110 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
112 : (double)height, (double)sink_rate,
113 : (double)gps.ground_speed(),
114 : (double)current_loc.get_distance(next_WP_loc));
115 : }
116 :
117 110 : type_slope_stage = SlopeStage::FINAL;
118 :
119 : #if AP_LANDINGGEAR_ENABLED
120 : // Check if the landing gear was deployed before landing
121 : // If not - go around
122 110 : AP_LandingGear *LG_inst = AP_LandingGear::get_singleton();
123 110 : if (LG_inst != nullptr && !LG_inst->check_before_land()) {
124 0 : type_slope_request_go_around();
125 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed");
126 : }
127 : #endif
128 : }
129 :
130 18153 : if (gps.ground_speed() < 3) {
131 : // reload any airspeed or groundspeed parameters that may have
132 : // been set for landing. We don't do this till ground
133 : // speed drops below 3.0 m/s as otherwise we will change
134 : // target speeds too early.
135 10191 : aparm.airspeed_cruise.load();
136 10191 : aparm.min_groundspeed.load();
137 10191 : aparm.throttle_cruise.load();
138 : }
139 37991 : } else if (type_slope_stage == SlopeStage::APPROACH && pre_flare_airspeed > 0) {
140 0 : bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt);
141 0 : bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec);
142 0 : if (reached_pre_flare_alt || reached_pre_flare_sec) {
143 0 : type_slope_stage = SlopeStage::PREFLARE;
144 : }
145 : }
146 :
147 : /*
148 : when landing we keep the L1 navigation waypoint 200m ahead. This
149 : prevents sudden turns if we overshoot the landing point
150 : */
151 37991 : Location land_WP_loc = next_WP_loc;
152 :
153 37991 : int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
154 37991 : land_WP_loc.offset_bearing(land_bearing_cd * 0.01f, prev_WP_loc.get_distance(current_loc) + 200);
155 37991 : nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
156 :
157 : // once landed and stationary, post some statistics
158 : // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
159 37991 : if (type_slope_flags.post_stats && !is_armed) {
160 110 : type_slope_flags.post_stats = false;
161 110 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc));
162 : }
163 :
164 : // check if we should auto-disarm after a confirmed landing
165 37991 : if (type_slope_stage == SlopeStage::FINAL) {
166 26143 : disarm_if_autoland_complete_fn();
167 : }
168 :
169 37991 : if (mission.continue_after_land() &&
170 0 : type_slope_stage == SlopeStage::FINAL &&
171 37991 : gps.status() >= AP_GPS_FixType::FIX_3D &&
172 0 : gps.ground_speed() < 1) {
173 : /*
174 : user has requested to continue with mission after a
175 : landing. Return true to allow for continue
176 : */
177 0 : return true;
178 : }
179 :
180 : /*
181 : we return false as a landing mission item never completes
182 :
183 : we stay on this waypoint unless the GCS commands us to change
184 : mission item, reset the mission, command a go-around or finish
185 : a land_abort procedure.
186 : */
187 37991 : return false;
188 : }
189 :
190 6097 : void AP_Landing::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)
191 : {
192 6097 : if (!rangefinder_state.in_use) {
193 6097 : return;
194 : }
195 :
196 : // check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
197 : // determining the slope from your current location to the land point then following that back up to the approach
198 : // altitude and moving the prev_wp to that location. From there
199 0 : float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
200 :
201 0 : if (slope_recalc_shallow_threshold <= 0 ||
202 0 : fabsf(correction_delta) < slope_recalc_shallow_threshold) {
203 0 : return;
204 : }
205 :
206 0 : rangefinder_state.last_stable_correction = rangefinder_state.correction;
207 :
208 0 : float corrected_alt_m = (adjusted_altitude_cm_fn() - loc_alt_AMSL_cm(next_WP_loc))*0.01f - rangefinder_state.correction;
209 0 : float total_distance_m = prev_WP_loc.get_distance(next_WP_loc);
210 0 : float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / wp_distance;
211 0 : prev_WP_loc.set_alt_cm(top_of_glide_slope_alt_m*100 + loc_alt_AMSL_cm(next_WP_loc), Location::AltFrame::ABSOLUTE);
212 :
213 : // re-calculate auto_state.land_slope with updated prev_WP_loc
214 0 : setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
215 :
216 0 : if (rangefinder_state.correction >= 0) { // we're too low or object is below us
217 : // correction positive means we're too low so we should continue on with
218 : // the newly computed shallower slope instead of pitching/throttling up
219 :
220 0 : } else if (slope_recalc_steep_threshold_to_abort > 0 && !type_slope_flags.has_aborted_due_to_slope_recalc) {
221 : // correction negative means we're too high and need to point down (and speed up) to re-align
222 : // to land on target. A large negative correction means we would have to dive down a lot and will
223 : // generating way too much speed that we can not bleed off in time. It is better to remember
224 : // the large baro altitude offset and abort the landing to come around again with the correct altitude
225 : // offset and "perfect" slope.
226 :
227 : // calculate projected slope with projected alt
228 0 : float new_slope_deg = degrees(atanf(slope));
229 0 : float initial_slope_deg = degrees(atanf(initial_slope));
230 :
231 : // is projected slope too steep?
232 0 : if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) {
233 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)",
234 : (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
235 0 : alt_offset = rangefinder_state.correction;
236 0 : flags.commanded_go_around = true;
237 0 : type_slope_flags.has_aborted_due_to_slope_recalc = true; // only allow this once.
238 0 : Log();
239 : }
240 : }
241 : }
242 :
243 2 : bool AP_Landing::type_slope_request_go_around(void)
244 : {
245 2 : flags.commanded_go_around = true;
246 2 : return true;
247 : }
248 :
249 : /*
250 : a special glide slope calculation for the landing approach
251 :
252 : During the land approach use a linear glide slope to a point
253 : projected through the landing point. We don't use the landing point
254 : itself as that leads to discontinuities close to the landing point,
255 : which can lead to erratic pitch control
256 : */
257 6097 : void AP_Landing::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)
258 : {
259 6097 : float total_distance = prev_WP_loc.get_distance(next_WP_loc);
260 :
261 : // If someone mistakenly puts all 0's in their LAND command then total_distance
262 : // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that.
263 6097 : if (total_distance < 1) {
264 0 : total_distance = 1;
265 : }
266 :
267 : // height we need to sink for this WP
268 6097 : float sink_height = (loc_alt_AMSL_cm(prev_WP_loc) - loc_alt_AMSL_cm(next_WP_loc))*0.01f;
269 :
270 : // current ground speed
271 6097 : float groundspeed = ahrs.groundspeed();
272 6097 : if (groundspeed < 0.5f) {
273 0 : groundspeed = 0.5f;
274 : }
275 :
276 : // calculate time to lose the needed altitude
277 6097 : float sink_time = total_distance / groundspeed;
278 6097 : if (sink_time < 0.5f) {
279 0 : sink_time = 0.5f;
280 : }
281 :
282 : // find the sink rate needed for the target location
283 6097 : float sink_rate = sink_height / sink_time;
284 :
285 : // the height we aim for is the one to give us the right flare point
286 6097 : float aim_height = flare_sec * sink_rate;
287 6097 : if (aim_height <= 0) {
288 0 : aim_height = flare_alt;
289 : }
290 :
291 : // don't allow the aim height to be too far above LAND_FLARE_ALT
292 6097 : if (flare_alt > 0 && aim_height > flare_alt*2) {
293 4094 : aim_height = flare_alt*2;
294 : }
295 :
296 : // calculate time spent in flare assuming the sink rate reduces over time from sink_rate at aim_height
297 : // to tecs_controller->get_land_sinkrate() at touchdown
298 6097 : const float weight = constrain_float(0.01f*(float)flare_effectivness_pct, 0.0f, 1.0f);
299 6097 : const float flare_sink_rate_avg = MAX(weight * tecs_Controller->get_land_sinkrate() + (1.0f - weight) * sink_rate, 0.1f);
300 6097 : const float flare_time = aim_height / flare_sink_rate_avg;
301 :
302 : // distance to flare is based on ground speed, adjusted as we
303 : // get closer. This takes into account the wind
304 6097 : float flare_distance = groundspeed * flare_time;
305 :
306 : // don't allow the flare before half way along the final leg
307 6097 : if (flare_distance > total_distance*0.5f) {
308 0 : flare_distance = total_distance*0.5f;
309 : }
310 :
311 : // project a point 500 meters past the landing point, passing
312 : // through the landing point
313 6097 : const float land_projection = 500;
314 6097 : int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
315 :
316 : // now calculate our aim point, which is before the landing
317 : // point and above it
318 6097 : Location loc = next_WP_loc;
319 6097 : loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
320 6097 : loc.offset_bearing(land_bearing_cd * 0.01f, -flare_distance);
321 6097 : loc.alt += aim_height*100;
322 :
323 : // calculate slope to landing point
324 6097 : bool is_first_calc = is_zero(slope);
325 6097 : slope = (sink_height - aim_height) / (total_distance - flare_distance);
326 6097 : if (is_first_calc) {
327 109 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope)));
328 : }
329 :
330 : // calculate point along that slope 500m ahead
331 6097 : loc.offset_bearing(land_bearing_cd * 0.01f, land_projection);
332 6097 : loc.offset_up_m(-slope * land_projection);
333 :
334 : // setup the offset_cm for set_target_altitude_proportion()
335 6097 : target_altitude_offset_cm = loc.alt - loc_alt_AMSL_cm(prev_WP_loc);
336 :
337 : // calculate the proportion we are to the target
338 6097 : float land_proportion = current_loc.line_path_proportion(prev_WP_loc, loc);
339 :
340 : // now setup the glide slope for landing
341 6097 : set_target_altitude_proportion_fn(loc, 1.0f - land_proportion);
342 :
343 : // stay within the range of the start and end locations in altitude
344 6097 : constrain_target_altitude_location_fn(loc, prev_WP_loc);
345 6097 : }
346 :
347 35442 : float AP_Landing::type_slope_get_target_airspeed_ms(void)
348 : {
349 : // we're landing, check for custom approach and
350 : // pre-flare airspeeds. Also increase for head-winds
351 :
352 : // default landing target airspeed is the average of cruise and
353 : // minimum airspeed:
354 35442 : float target_airspeed_ms = 0.5f * (aparm.airspeed_cruise + aparm.airspeed_min);
355 :
356 : // land airspeed can be explicitly specified:
357 35442 : const float land_airspeed = tecs_Controller->get_land_airspeed();
358 35442 : if (land_airspeed >= 0) {
359 370 : target_airspeed_ms = land_airspeed;
360 : }
361 :
362 35442 : switch (type_slope_stage) {
363 5861 : case SlopeStage::NORMAL:
364 5861 : target_airspeed_ms = aparm.airspeed_cruise;
365 5861 : break;
366 6097 : case SlopeStage::APPROACH:
367 6097 : break;
368 23484 : case SlopeStage::PREFLARE:
369 : case SlopeStage::FINAL:
370 23484 : if (pre_flare_airspeed > 0) {
371 : // if we just preflared then continue using the pre-flare airspeed during final flare
372 0 : target_airspeed_ms = pre_flare_airspeed;
373 : }
374 23484 : break;
375 : }
376 :
377 : // when landing, add scaled amount of head-wind based on a
378 : // percentage-parameter
379 35442 : const float head_wind_comp = constrain_float(wind_comp, 0.0f, 100.0f)*0.01;
380 35442 : const float head_wind_compensation_ms = ahrs.head_wind() * head_wind_comp;
381 :
382 35442 : const float max_airspeed_ms = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max : aparm.airspeed_cruise;
383 :
384 35442 : return constrain_float(target_airspeed_ms + head_wind_compensation_ms, target_airspeed_ms, max_airspeed_ms);
385 : }
386 :
387 203106 : int32_t AP_Landing::type_slope_constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd)
388 : {
389 203106 : if (type_slope_stage == SlopeStage::FINAL) {
390 135720 : return constrain_int32(desired_roll_cd, level_roll_limit_cd * -1, level_roll_limit_cd);
391 : } else {
392 67386 : return desired_roll_cd;
393 : }
394 : }
395 :
396 858102 : bool AP_Landing::type_slope_is_flaring(void) const
397 : {
398 858102 : return (type_slope_stage == SlopeStage::FINAL);
399 : }
400 :
401 176746 : bool AP_Landing::type_slope_is_on_final(void) const
402 : {
403 353492 : return (type_slope_stage == SlopeStage::PREFLARE ||
404 353492 : type_slope_stage == SlopeStage::FINAL);
405 : }
406 :
407 251769 : bool AP_Landing::type_slope_is_on_approach(void) const
408 : {
409 424234 : return (type_slope_stage == SlopeStage::APPROACH ||
410 424234 : type_slope_stage == SlopeStage::PREFLARE);
411 : }
412 :
413 690 : bool AP_Landing::type_slope_is_expecting_impact(void) const
414 : {
415 690 : return type_slope_is_on_final();
416 : }
417 :
418 0 : bool AP_Landing::type_slope_is_complete(void) const
419 : {
420 0 : return (type_slope_stage == SlopeStage::FINAL);
421 : }
422 :
423 : #if HAL_LOGGING_ENABLED
424 54686 : void AP_Landing::type_slope_log(void) const
425 : {
426 : // @LoggerMessage: LAND
427 : // @Description: Slope Landing data
428 : // @Field: TimeUS: Time since system startup
429 : // @Field: stage: progress through landing sequence
430 : // @Field: f1: Landing flags
431 : // @Field: f2: Slope-specific landing flags
432 : // @Field: slope: Slope to landing point
433 : // @Field: slopeInit: Initial slope to landing point
434 : // @Field: altO: Rangefinder correction
435 : // @Field: fh: Height for flare timing.
436 109372 : AP::logger().WriteStreaming("LAND", "TimeUS,stage,f1,f2,slope,slopeInit,altO,fh", "QBBBffff",
437 : AP_HAL::micros64(),
438 : type_slope_stage,
439 : flags,
440 : type_slope_flags,
441 54686 : (double)slope,
442 54686 : (double)initial_slope,
443 54686 : (double)alt_offset,
444 54686 : (double)height_flare_log);
445 54686 : }
446 : #endif
447 :
448 386342 : bool AP_Landing::type_slope_is_throttle_suppressed(void) const
449 : {
450 386342 : return type_slope_stage == SlopeStage::FINAL;
451 : }
|
