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 : #include "Plane.h"
17 :
18 : /*
19 : altitude handling routines. These cope with both barometric control
20 : and terrain following control
21 : */
22 :
23 : /*
24 : adjust altitude target depending on mode
25 : */
26 550966 : void Plane::adjust_altitude_target()
27 : {
28 550966 : control_mode->update_target_altitude();
29 550966 : }
30 :
31 511367 : void Plane::check_home_alt_change(void)
32 : {
33 511367 : int32_t home_alt_cm = ahrs.get_home().alt;
34 511367 : if (home_alt_cm != auto_state.last_home_alt_cm && hal.util->get_soft_armed()) {
35 : // cope with home altitude changing
36 291 : const int32_t alt_change_cm = home_alt_cm - auto_state.last_home_alt_cm;
37 291 : fix_terrain_WP(next_WP_loc, __AP_LINE__);
38 :
39 : // reset TECS to force the field elevation estimate to reset
40 291 : TECS_controller.offset_altitude(alt_change_cm * 0.01f);
41 : }
42 511367 : auto_state.last_home_alt_cm = home_alt_cm;
43 511367 : }
44 :
45 : /*
46 : setup for a gradual altitude slope to the next waypoint, if appropriate
47 : */
48 4166 : void Plane::setup_alt_slope(void)
49 : {
50 : // establish the distance we are travelling to the next waypoint,
51 : // for calculating out rate of change of altitude
52 4166 : auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
53 4166 : auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
54 4166 : TECS_controller.set_path_proportion(auto_state.wp_proportion);
55 4166 : update_flight_stage();
56 :
57 : /*
58 : work out if we will gradually change altitude, or try to get to
59 : the new altitude as quickly as possible.
60 : */
61 4166 : switch (control_mode->mode_number()) {
62 : #if MODE_AUTOLAND_ENABLED
63 284 : case Mode::Number::AUTOLAND:
64 : #endif
65 : case Mode::Number::RTL:
66 : case Mode::Number::AVOID_ADSB:
67 : case Mode::Number::GUIDED:
68 : /* glide down slowly if above target altitude, but ascend more
69 : rapidly if below it. See
70 : https://github.com/ArduPilot/ardupilot/issues/39
71 : */
72 284 : if (above_location_current(next_WP_loc)) {
73 86 : set_offset_altitude_location(prev_WP_loc, next_WP_loc);
74 : } else {
75 198 : reset_offset_altitude();
76 : }
77 284 : break;
78 :
79 3842 : case Mode::Number::AUTO:
80 : // climb without doing slope if option is enabled
81 3842 : if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) {
82 0 : reset_offset_altitude();
83 0 : break;
84 : }
85 :
86 : // otherwise we set up an altitude slope for this leg
87 3842 : set_offset_altitude_location(prev_WP_loc, next_WP_loc);
88 :
89 3842 : break;
90 40 : default:
91 40 : reset_offset_altitude();
92 40 : break;
93 : }
94 4166 : }
95 :
96 : /*
97 : return RTL altitude as AMSL cm
98 : */
99 69 : int32_t Plane::get_RTL_altitude_cm() const
100 : {
101 69 : if (g.RTL_altitude < 0) {
102 0 : return current_loc.alt;
103 : }
104 69 : return g.RTL_altitude*100 + home.alt;
105 : }
106 :
107 : /*
108 : return relative altitude in meters (relative to terrain, if available,
109 : or home otherwise)
110 : */
111 3116016 : float Plane::relative_ground_altitude(enum RangeFinderUse use_rangefinder, bool use_terrain_if_available)
112 : {
113 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
114 : float height_AGL;
115 : // use external HAGL if available
116 3116016 : if (get_external_HAGL(height_AGL)) {
117 0 : return height_AGL;
118 : }
119 : #endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
120 :
121 : #if AP_RANGEFINDER_ENABLED
122 3116016 : if (rangefinder_use(use_rangefinder) && rangefinder_state.in_range) {
123 113690 : return rangefinder_state.height_estimate;
124 : }
125 : #endif
126 :
127 : #if HAL_QUADPLANE_ENABLED && AP_RANGEFINDER_ENABLED
128 3002326 : if (rangefinder_use(use_rangefinder) && quadplane.in_vtol_land_final() &&
129 0 : rangefinder.status_orient(rangefinder_orientation()) == RangeFinder::Status::OutOfRangeLow) {
130 : // a special case for quadplane landing when rangefinder goes
131 : // below minimum. Consider our height above ground to be zero
132 0 : return 0;
133 : }
134 : #endif
135 :
136 : #if AP_TERRAIN_AVAILABLE
137 : float altitude;
138 506922 : if (use_terrain_if_available &&
139 3509248 : terrain.status() == AP_Terrain::TerrainStatusOK &&
140 506922 : terrain.height_above_terrain(altitude, true)) {
141 506922 : return altitude;
142 : }
143 : #endif
144 :
145 : #if HAL_QUADPLANE_ENABLED
146 3575242 : if (quadplane.in_vtol_land_descent() &&
147 1079838 : !quadplane.landing_with_fixed_wing_spiral_approach()) {
148 : // when doing a VTOL landing we can use the waypoint height as
149 : // ground height. We can't do this if using the
150 : // LAND_FW_APPROACH as that uses the wp height as the approach
151 : // height
152 1033134 : return height_above_target();
153 : }
154 : #endif
155 :
156 1462270 : return relative_altitude;
157 : }
158 :
159 : /*
160 : return true if we should use the rangefinder for a specific use case
161 : */
162 6226560 : bool Plane::rangefinder_use(enum RangeFinderUse use_rangefinder) const
163 : {
164 6226560 : const uint8_t use = uint8_t(g.rangefinder_landing.get());
165 6226560 : if (use == uint8_t(RangeFinderUse::NONE)) {
166 5943845 : return false;
167 : }
168 282715 : if (use & uint8_t(RangeFinderUse::ALL)) {
169 : // if ALL bit is set then ignore other bits
170 282715 : return true;
171 : }
172 0 : return (use & uint8_t(use_rangefinder)) != 0;
173 : }
174 :
175 : // Helper for above method using terrain if the vehicle is currently terrain following
176 3092962 : float Plane::relative_ground_altitude(enum RangeFinderUse use_rangefinder)
177 : {
178 : #if AP_TERRAIN_AVAILABLE
179 3092962 : return relative_ground_altitude(use_rangefinder, target_altitude.terrain_following);
180 : #else
181 : return relative_ground_altitude(use_rangefinder, false);
182 : #endif
183 : }
184 :
185 :
186 : /*
187 : set the target altitude to the current altitude. This is used when
188 : setting up for altitude hold, such as when releasing elevator in
189 : CRUISE mode.
190 : */
191 8131 : void Plane::set_target_altitude_current(void)
192 : {
193 : // record altitude above sea level at the current time as our
194 : // target altitude
195 8131 : target_altitude.amsl_cm = current_loc.alt;
196 :
197 : // reset any altitude slope offset
198 8131 : reset_offset_altitude();
199 :
200 : #if AP_TERRAIN_AVAILABLE
201 : // also record the terrain altitude if possible
202 : float terrain_altitude;
203 8131 : if (terrain_enabled_in_current_mode() && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) {
204 16 : target_altitude.terrain_following = true;
205 16 : target_altitude.terrain_alt_cm = terrain_altitude*100;
206 : } else {
207 : // if terrain following is disabled, or we don't know our
208 : // terrain altitude when we set the altitude then don't
209 : // terrain follow
210 8115 : target_altitude.terrain_following = false;
211 : }
212 : #endif
213 8131 : }
214 :
215 : /*
216 : set target altitude based on a location structure
217 : */
218 536260 : void Plane::set_target_altitude_location(const Location &loc)
219 : {
220 536260 : target_altitude.amsl_cm = loc.alt;
221 536260 : if (loc.relative_alt) {
222 74656 : target_altitude.amsl_cm += home.alt;
223 : }
224 : #if AP_TERRAIN_AVAILABLE
225 536260 : if (target_altitude.terrain_following_pending) {
226 : /* we didn't get terrain data to init when we started on this
227 : target, retry
228 : */
229 0 : setup_terrain_target_alt(next_WP_loc);
230 : }
231 : /*
232 : if this location has the terrain_alt flag set and we know the
233 : terrain altitude of our current location then treat it as a
234 : terrain altitude
235 : */
236 : float height;
237 536260 : if (loc.terrain_alt && terrain.height_above_terrain(height, true)) {
238 71596 : target_altitude.terrain_following = true;
239 71596 : target_altitude.terrain_alt_cm = loc.alt;
240 : } else {
241 464664 : target_altitude.terrain_following = false;
242 : }
243 : #endif
244 536260 : }
245 :
246 : /*
247 : return relative to home target altitude in centimeters. Used for
248 : altitude control libraries
249 : */
250 313893 : int32_t Plane::relative_target_altitude_cm(void)
251 : {
252 : #if AP_TERRAIN_AVAILABLE
253 : float relative_home_height;
254 373431 : if (target_altitude.terrain_following &&
255 59538 : terrain.height_relative_home_equivalent(target_altitude.terrain_alt_cm*0.01f,
256 : relative_home_height, true)) {
257 : // add lookahead adjustment the target altitude
258 59538 : target_altitude.lookahead = lookahead_adjustment();
259 59538 : relative_home_height += target_altitude.lookahead;
260 :
261 : #if AP_RANGEFINDER_ENABLED
262 : // correct for rangefinder data
263 59538 : relative_home_height += rangefinder_correction();
264 : #endif
265 :
266 : // we are following terrain, and have terrain data for the
267 : // current location. Use it.
268 59538 : return relative_home_height*100;
269 : }
270 : #endif
271 254355 : int32_t relative_alt = target_altitude.amsl_cm - home.alt;
272 254355 : relative_alt += mission_alt_offset()*100;
273 : #if AP_RANGEFINDER_ENABLED
274 254355 : relative_alt += rangefinder_correction() * 100;
275 : #endif
276 254355 : return relative_alt;
277 : }
278 :
279 : /*
280 : change the current target altitude by an amount in centimeters. Used
281 : to cope with changes due to elevator in CRUISE or FBWB
282 : */
283 92318 : void Plane::change_target_altitude(int32_t change_cm)
284 : {
285 92318 : target_altitude.amsl_cm += change_cm;
286 : #if AP_TERRAIN_AVAILABLE
287 92318 : if (target_altitude.terrain_following && !terrain_disabled()) {
288 925 : target_altitude.terrain_alt_cm += change_cm;
289 : }
290 : #endif
291 92318 : }
292 :
293 : /*
294 : change target altitude by a proportion of the target altitude offset
295 : (difference in height to next WP from previous WP). proportion
296 : should be between 0 and 1.
297 :
298 : When proportion is zero we have reached the destination. When
299 : proportion is 1 we are at the starting waypoint.
300 :
301 : Note that target_altitude is setup initially based on the
302 : destination waypoint
303 : */
304 75179 : void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
305 : {
306 75179 : set_target_altitude_location(loc);
307 :
308 : // Only do altitude slope handling when above CLIMB_SLOPE_HGT or when
309 : // descending. This is meant to prevent situations where the aircraft tries
310 : // to slowly gain height at low altitudes, potentially hitting obstacles.
311 86657 : if (target_altitude.offset_cm > 0 &&
312 11478 : (adjusted_relative_altitude_cm() <
313 11478 : (g2.waypoint_climb_slope_height_min * 100))) {
314 : // Early return to ensure a full-rate climb past CLIMB_SLOPE_HGT
315 2385 : return;
316 : }
317 :
318 72794 : proportion = constrain_float(proportion, 0.0f, 1.0f);
319 72794 : change_target_altitude(-target_altitude.offset_cm*proportion);
320 :
321 : // rebuild the altitude slope if we are above it and supposed to be climbing
322 72794 : if (g.alt_slope_max_height > 0) {
323 72055 : if (target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.alt_slope_max_height) {
324 88 : set_target_altitude_location(loc);
325 88 : set_offset_altitude_location(current_loc, loc);
326 88 : change_target_altitude(-target_altitude.offset_cm*proportion);
327 : // adjust the new target offset altitude to reflect that we are partially already done
328 88 : if (proportion > 0.0f)
329 88 : target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
330 : }
331 : }
332 : }
333 :
334 : #if AP_TERRAIN_AVAILABLE
335 : /*
336 : change target altitude along a path between two locations
337 : (prev_WP_loc and next_WP_loc) where the second location is a terrain
338 : altitude
339 : */
340 58495 : bool Plane::set_target_altitude_proportion_terrain(void)
341 : {
342 58495 : if (!next_WP_loc.terrain_alt ||
343 58495 : !next_WP_loc.relative_alt) {
344 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
345 0 : return false;
346 : }
347 : /*
348 : we first need to get the height of the terrain at prev_WP_loc
349 : */
350 : float prev_WP_height_terrain;
351 58495 : if (!plane.prev_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN,
352 : prev_WP_height_terrain)) {
353 0 : return false;
354 : }
355 : // and next_WP_loc alt as terrain
356 : float next_WP_height_terrain;
357 58495 : if (!plane.next_WP_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN,
358 : next_WP_height_terrain)) {
359 0 : return false;
360 : }
361 58495 : Location loc = next_WP_loc;
362 58495 : const auto alt = linear_interpolate(prev_WP_height_terrain, next_WP_height_terrain,
363 : plane.auto_state.wp_proportion, 0, 1);
364 :
365 58495 : loc.set_alt_m(alt, Location::AltFrame::ABOVE_TERRAIN);
366 :
367 58495 : set_target_altitude_location(loc);
368 :
369 58495 : return true;
370 : }
371 : #endif // AP_TERRAIN_AVAILABLE
372 :
373 : /*
374 : constrain target altitude to be between two locations. Used to
375 : ensure we stay within two waypoints in altitude
376 : */
377 75179 : void Plane::constrain_target_altitude_location(const Location &loc1, const Location &loc2)
378 : {
379 75179 : if (loc1.alt > loc2.alt) {
380 18376 : target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt);
381 : } else {
382 56803 : target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc1.alt, loc2.alt);
383 : }
384 75179 : }
385 :
386 : /*
387 : return error between target altitude and current altitude
388 : */
389 1945976 : int32_t Plane::calc_altitude_error_cm(void)
390 : {
391 : #if AP_TERRAIN_AVAILABLE
392 : float terrain_height;
393 2232335 : if (target_altitude.terrain_following &&
394 286359 : terrain.height_above_terrain(terrain_height, true)) {
395 286359 : return target_altitude.lookahead*100 + target_altitude.terrain_alt_cm - (terrain_height*100);
396 : }
397 : #endif
398 1659617 : return target_altitude.amsl_cm - adjusted_altitude_cm();
399 : }
400 :
401 : /*
402 : check for cruise_alt_floor and fence min/max altitude
403 : */
404 195631 : void Plane::check_fbwb_altitude(void)
405 : {
406 195631 : float max_alt_cm = 0.0;
407 195631 : float min_alt_cm = 0.0;
408 195631 : bool should_check_max = false;
409 195631 : bool should_check_min = false;
410 :
411 : #if AP_FENCE_ENABLED
412 : // taking fence max and min altitude (with margin)
413 195631 : const uint8_t enabled_fences = plane.fence.get_enabled_fences();
414 195631 : if ((enabled_fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
415 493 : min_alt_cm = plane.fence.get_relative_safe_alt_min_m()*100.0;
416 493 : should_check_min = true;
417 : }
418 195631 : if ((enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
419 0 : max_alt_cm = plane.fence.get_relative_safe_alt_max_m()*100.0;
420 0 : should_check_max = true;
421 : }
422 : #endif
423 :
424 195631 : if (g.cruise_alt_floor > 0) {
425 : // FBWB min altitude exists
426 0 : min_alt_cm = MAX(min_alt_cm, plane.g.cruise_alt_floor*100.0);
427 0 : should_check_min = true;
428 : }
429 :
430 195631 : if (!should_check_min && !should_check_max) {
431 195138 : return;
432 : }
433 :
434 : //check if terrain following (min and max)
435 : #if AP_TERRAIN_AVAILABLE
436 493 : if (target_altitude.terrain_following) {
437 : // set our target terrain height to be at least the min set
438 0 : if (should_check_max) {
439 0 : target_altitude.terrain_alt_cm = MIN(target_altitude.terrain_alt_cm, max_alt_cm);
440 : }
441 0 : if (should_check_min) {
442 0 : target_altitude.terrain_alt_cm = MAX(target_altitude.terrain_alt_cm, min_alt_cm);
443 : }
444 0 : return;
445 : }
446 : #endif
447 :
448 493 : if (should_check_max) {
449 0 : target_altitude.amsl_cm = MIN(target_altitude.amsl_cm, home.alt + max_alt_cm);
450 : }
451 493 : if (should_check_min) {
452 493 : target_altitude.amsl_cm = MAX(target_altitude.amsl_cm, home.alt + min_alt_cm);
453 : }
454 : }
455 :
456 : /*
457 : reset the altitude offset used for altitude slopes
458 : */
459 16567 : void Plane::reset_offset_altitude(void)
460 : {
461 16567 : target_altitude.offset_cm = 0;
462 16567 : }
463 :
464 :
465 : /*
466 : reset the altitude offset used for slopes, based on difference between
467 : altitude at a destination and a specified start altitude. If destination is
468 : above the starting altitude then the result is positive.
469 : */
470 4016 : void Plane::set_offset_altitude_location(const Location &start_loc, const Location &destination_loc)
471 : {
472 4016 : ftype alt_difference_m = 0;
473 4016 : if (destination_loc.get_height_above(start_loc, alt_difference_m)) {
474 3997 : target_altitude.offset_cm = alt_difference_m * 100;
475 : } else {
476 19 : target_altitude.offset_cm = 0;
477 : }
478 :
479 : #if AP_TERRAIN_AVAILABLE
480 : /*
481 : if this location has the terrain_alt flag set and we know the
482 : terrain altitude of our current location then treat it as a
483 : terrain altitude
484 : */
485 : float height;
486 8155 : if (destination_loc.terrain_alt &&
487 4097 : target_altitude.terrain_following &&
488 81 : terrain.height_above_terrain(height, true)) {
489 81 : target_altitude.offset_cm = target_altitude.terrain_alt_cm - (height * 100);
490 : }
491 : #endif
492 :
493 4016 : if (flight_stage != AP_FixedWing::FlightStage::LAND) {
494 : // if we are within ALT_SLOPE_MIN meters of the target altitude then
495 : // reset the offset to not use an altitude slope. This allows for more
496 : // accurate flight of missions where the aircraft may lose or gain a bit
497 : // of altitude near waypoint turn points due to local terrain changes
498 7810 : if (g.alt_slope_min <= 0 ||
499 3905 : labs(target_altitude.offset_cm)*0.01f < g.alt_slope_min) {
500 1180 : target_altitude.offset_cm = 0;
501 : }
502 : }
503 4016 : }
504 :
505 : /*
506 : return true if current_loc is above loc. Used for altitude slope
507 : calculations.
508 :
509 : "above" is simple if we are not terrain following, as it just means
510 : the pressure altitude of one is above the other.
511 :
512 : When in terrain following mode "above" means the over-the-terrain
513 : current altitude is above the over-the-terrain alt of loc. It is
514 : quite possible for current_loc to be "above" loc when it is at a
515 : lower pressure altitude, if current_loc is in a low part of the
516 : terrain
517 : */
518 4126 : bool Plane::above_location_current(const Location &loc)
519 : {
520 : #if AP_TERRAIN_AVAILABLE
521 : float terrain_alt;
522 4266 : if (loc.terrain_alt &&
523 140 : terrain.height_above_terrain(terrain_alt, true)) {
524 140 : float loc_alt = loc.alt*0.01f;
525 140 : if (!loc.relative_alt) {
526 0 : loc_alt -= home.alt*0.01f;
527 : }
528 140 : return terrain_alt > loc_alt;
529 : }
530 : #endif
531 :
532 3986 : float loc_alt_cm = loc.alt;
533 3986 : if (loc.relative_alt) {
534 12 : loc_alt_cm += home.alt;
535 : }
536 3986 : return current_loc.alt > loc_alt_cm;
537 : }
538 :
539 : /*
540 : modify a destination to be setup for terrain following if
541 : TERRAIN_FOLLOW is enabled
542 : */
543 133 : void Plane::setup_terrain_target_alt(Location &loc)
544 : {
545 : #if AP_TERRAIN_AVAILABLE
546 133 : if (terrain_enabled_in_current_mode()) {
547 4 : if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
548 0 : target_altitude.terrain_following_pending = true;
549 0 : return;
550 : }
551 : }
552 133 : target_altitude.terrain_following_pending = false;
553 : #endif
554 : }
555 :
556 : /*
557 : return current_loc.alt adjusted for ALT_OFFSET
558 : This is useful during long flights to account for barometer changes
559 : from the GCS, or to adjust the flying height of a long mission
560 : */
561 2792490 : int32_t Plane::adjusted_altitude_cm(void)
562 : {
563 2792490 : return current_loc.alt - (mission_alt_offset()*100);
564 : }
565 :
566 : /*
567 : return home-relative altitude adjusted for ALT_OFFSET. This is useful
568 : during long flights to account for barometer changes from the GCS,
569 : or to adjust the flying height of a long mission.
570 : */
571 83762 : int32_t Plane::adjusted_relative_altitude_cm(void)
572 : {
573 83762 : return (relative_altitude - mission_alt_offset())*100;
574 : }
575 :
576 :
577 : /*
578 : return the mission altitude offset. This raises or lowers all
579 : mission items. It is primarily set using the ALT_OFFSET parameter,
580 : but can also be adjusted by the rangefinder landing code for a
581 : NAV_LAND command if we have aborted a steep landing
582 : */
583 3130607 : float Plane::mission_alt_offset(void)
584 : {
585 3130607 : float ret = g.alt_offset;
586 3130607 : if (control_mode == &mode_auto &&
587 1830719 : (flight_stage == AP_FixedWing::FlightStage::LAND || auto_state.wp_is_land_approach)) {
588 : // when landing after an aborted landing due to too high glide
589 : // slope we use an offset from the last landing attempt
590 268863 : ret += landing.alt_offset;
591 : }
592 3130607 : return ret;
593 : }
594 :
595 3684 : void Plane::reset_alt_offset(bool force)
596 : {
597 3684 : if (!force && !flight_option_enabled(FlightOptions::RESET_ALT_OFFSET)) {
598 2987 : return;
599 : }
600 :
601 697 : if (g.alt_offset == 0) {
602 689 : return;
603 : }
604 :
605 8 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reset ALT_OFFSET");
606 8 : g.alt_offset.set(0);
607 : }
608 :
609 : /*
610 : return the height in meters above the next_WP_loc altitude
611 : */
612 1156434 : float Plane::height_above_target(void)
613 : {
614 1156434 : float target_alt = next_WP_loc.alt*0.01;
615 1156434 : if (!next_WP_loc.relative_alt) {
616 1132873 : target_alt -= ahrs.get_home().alt*0.01f;
617 : }
618 :
619 : #if AP_TERRAIN_AVAILABLE
620 : // also record the terrain altitude if possible
621 : float terrain_altitude;
622 1179995 : if (next_WP_loc.terrain_alt &&
623 23561 : terrain.height_above_terrain(terrain_altitude, true)) {
624 23561 : return terrain_altitude - target_alt;
625 : }
626 : #endif
627 :
628 1132873 : return (adjusted_altitude_cm()*0.01f - ahrs.get_home().alt*0.01f) - target_alt;
629 : }
630 :
631 : /*
632 : work out target altitude adjustment from terrain lookahead
633 : */
634 59538 : float Plane::lookahead_adjustment(void)
635 : {
636 : #if AP_TERRAIN_AVAILABLE
637 : int32_t bearing_cd;
638 : int16_t distance;
639 : // work out distance and bearing to target
640 59538 : if (control_mode == &mode_fbwb) {
641 : // there is no target waypoint in FBWB, so use yaw as an approximation
642 0 : bearing_cd = ahrs.yaw_sensor;
643 0 : distance = g.terrain_lookahead;
644 59538 : } else if (!reached_loiter_target()) {
645 48615 : bearing_cd = nav_controller->target_bearing_cd();
646 48615 : distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead);
647 : } else {
648 : // no lookahead when loitering
649 10923 : bearing_cd = 0;
650 10923 : distance = 0;
651 : }
652 59538 : if (distance <= 0) {
653 : // no lookahead
654 11850 : return 0;
655 : }
656 :
657 :
658 47688 : float groundspeed = ahrs.groundspeed();
659 47688 : if (groundspeed < 1) {
660 : // we're not moving
661 170 : return 0;
662 : }
663 : // we need to know the climb ratio. We use 50% of the maximum
664 : // climb rate so we are not constantly at 100% throttle and to
665 : // give a bit more margin on terrain
666 47518 : float climb_ratio = 0.5f * TECS_controller.get_max_climbrate() / groundspeed;
667 :
668 47518 : if (climb_ratio <= 0) {
669 : // lookahead makes no sense for negative climb rates
670 0 : return 0;
671 : }
672 :
673 : // ask the terrain code for the lookahead altitude change
674 47518 : float lookahead = terrain.lookahead(bearing_cd*0.01f, distance, climb_ratio);
675 :
676 47518 : if (target_altitude.offset_cm < 0) {
677 : // we are heading down to the waypoint, so we don't need to
678 : // climb as much
679 11951 : lookahead += target_altitude.offset_cm*0.01f;
680 : }
681 :
682 : // constrain lookahead to a reasonable limit
683 47518 : return constrain_float(lookahead, 0, 1000.0f);
684 : #else
685 : return 0;
686 : #endif
687 : }
688 :
689 :
690 : #if AP_RANGEFINDER_ENABLED
691 : /*
692 : correct target altitude using rangefinder data. Returns offset in
693 : meters to correct target altitude. A positive number means we need
694 : to ask the speed/height controller to fly higher
695 : */
696 388885 : float Plane::rangefinder_correction(void)
697 : {
698 388885 : if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
699 : // we haven't had any rangefinder data for 5s - don't use it
700 382198 : return 0;
701 : }
702 :
703 : // for now we only support the rangefinder for landing
704 6687 : bool using_rangefinder = (rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) &&
705 6687 : flight_stage == AP_FixedWing::FlightStage::LAND &&
706 0 : rangefinder_state.in_use);
707 6687 : if (!using_rangefinder) {
708 6687 : return 0;
709 : }
710 :
711 0 : return rangefinder_state.correction;
712 : }
713 :
714 : /*
715 : correct rangefinder data for terrain height difference between
716 : NAV_LAND point and current location
717 : */
718 53630 : void Plane::rangefinder_terrain_correction(float &height)
719 : {
720 : #if AP_TERRAIN_AVAILABLE
721 53630 : if (!rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) ||
722 53630 : flight_stage != AP_FixedWing::FlightStage::LAND ||
723 0 : !terrain_enabled_in_current_mode()) {
724 53630 : return;
725 : }
726 : float terrain_amsl1, terrain_amsl2;
727 0 : if (!terrain.height_amsl(current_loc, terrain_amsl1) ||
728 0 : !terrain.height_amsl(next_WP_loc, terrain_amsl2)) {
729 0 : return;
730 : }
731 0 : float correction = (terrain_amsl1 - terrain_amsl2);
732 0 : height += correction;
733 0 : auto_state.terrain_correction = correction;
734 : #endif
735 : }
736 :
737 : /*
738 : update the offset between rangefinder height and terrain height
739 : */
740 2736341 : void Plane::rangefinder_height_update(void)
741 : {
742 2736341 : const auto orientation = rangefinder_orientation();
743 2736341 : bool range_ok = rangefinder.status_orient(orientation) == RangeFinder::Status::Good;
744 2736341 : float distance = rangefinder.distance_orient(orientation);
745 2736341 : float corrected_distance = distance;
746 :
747 : /*
748 : correct distance for attitude
749 : */
750 2736341 : if (range_ok) {
751 : // correct the range for attitude
752 56395 : const auto &dcm = ahrs.get_rotation_body_to_ned();
753 :
754 56395 : Vector3f v{corrected_distance, 0, 0};
755 56395 : v.rotate(orientation);
756 56395 : v = dcm * v;
757 :
758 56395 : if (!is_positive(v.z)) {
759 : // not pointing at the ground
760 5 : range_ok = false;
761 : } else {
762 56390 : corrected_distance = v.z;
763 : }
764 : }
765 :
766 2736341 : if (range_ok && ahrs.home_is_set()) {
767 53630 : if (!rangefinder_state.have_initial_reading) {
768 9 : rangefinder_state.have_initial_reading = true;
769 9 : rangefinder_state.initial_range = distance;
770 : }
771 53630 : rangefinder_state.height_estimate = corrected_distance;
772 :
773 53630 : rangefinder_terrain_correction(rangefinder_state.height_estimate);
774 :
775 : // we consider ourselves to be fully in range when we have 10
776 : // good samples (0.2s) that are different by 5% of the maximum
777 : // range from the initial range we see. The 5% change is to
778 : // catch Lidars that are giving a constant range, either due
779 : // to misconfiguration or a faulty sensor
780 53630 : if (rangefinder_state.in_range_count < 10) {
781 6472 : if (!is_equal(distance, rangefinder_state.last_distance) &&
782 1150 : fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_orient(rangefinder_orientation())) {
783 320 : rangefinder_state.in_range_count++;
784 : }
785 5322 : if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_orient(rangefinder_orientation())*0.2) {
786 : // changes by more than 20% of full range will reset counter
787 2 : rangefinder_state.in_range_count = 0;
788 : }
789 : } else {
790 48308 : rangefinder_state.in_range = true;
791 48308 : bool flightstage_good_for_rangefinder_landing = false;
792 48308 : if (flight_stage == AP_FixedWing::FlightStage::LAND) {
793 592 : flightstage_good_for_rangefinder_landing = true;
794 : }
795 : #if HAL_QUADPLANE_ENABLED
796 142413 : if (control_mode == &mode_qland ||
797 91294 : control_mode == &mode_qrtl ||
798 42986 : (control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) {
799 14341 : flightstage_good_for_rangefinder_landing = true;
800 : }
801 : #endif
802 :
803 : // Check if the aircraft is within RNGFND_LND_DIST meters from the
804 : // landing point to engage it
805 48308 : const int16_t land_engage_dist_m = g2.rangefinder_land_engage_dist_m;
806 48308 : const bool is_within_engagement_distance =
807 48308 : (land_engage_dist_m <= 0) ||
808 0 : (auto_state.wp_distance <= land_engage_dist_m);
809 :
810 40664 : if (!rangefinder_state.in_use &&
811 7289 : flightstage_good_for_rangefinder_landing &&
812 96261 : rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) &&
813 : is_within_engagement_distance) {
814 3 : rangefinder_state.in_use = true;
815 3 : gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
816 : }
817 : }
818 53630 : rangefinder_state.last_distance = distance;
819 : } else {
820 2682711 : rangefinder_state.in_range_count = 0;
821 2682711 : rangefinder_state.in_range = false;
822 : }
823 :
824 2736341 : if (rangefinder_state.in_range) {
825 : // If not using terrain data, we expect zero correction when our height above target is equal to our rangefinder measurement
826 48308 : float correction = height_above_target() - rangefinder_state.height_estimate;
827 :
828 : #if AP_TERRAIN_AVAILABLE
829 : // if we are terrain following then correction is based on terrain data
830 : float terrain_altitude;
831 84047 : if ((target_altitude.terrain_following || terrain_enabled_in_current_mode()) &&
832 35739 : terrain.height_above_terrain(terrain_altitude, true)) {
833 35739 : correction = terrain_altitude - rangefinder_state.height_estimate;
834 : }
835 : #endif
836 :
837 : // remember the last correction. Use a low pass filter unless
838 : // the old data is more than 5 seconds old
839 48308 : uint32_t now = millis();
840 48308 : if (now - rangefinder_state.last_correction_time_ms > 5000) {
841 24 : rangefinder_state.correction = correction;
842 24 : rangefinder_state.initial_correction = correction;
843 24 : if (rangefinder_use(RangeFinderUse::TAKEOFF_LANDING)) {
844 7 : landing.set_initial_slope();
845 : }
846 24 : rangefinder_state.last_correction_time_ms = now;
847 : } else {
848 48284 : rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
849 48284 : rangefinder_state.last_correction_time_ms = now;
850 48284 : if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {
851 : // the correction has changed by more than 30m, reset use of Lidar. We may have a bad lidar
852 1 : if (rangefinder_state.in_use) {
853 0 : gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate);
854 : }
855 1 : memset(&rangefinder_state, 0, sizeof(rangefinder_state));
856 : }
857 : }
858 :
859 : }
860 2736341 : }
861 : #endif // AP_RANGEFINDER_ENABLED
862 :
863 : /*
864 : determine if Non Auto Terrain Disable is active and allowed in present control mode
865 : */
866 941 : bool Plane::terrain_disabled()
867 : {
868 941 : return control_mode->allows_terrain_disable() && non_auto_terrain_disable;
869 : }
870 :
871 :
872 : /*
873 : Check if terrain following is enabled for the current mode
874 : */
875 : #if AP_TERRAIN_AVAILABLE
876 : const Plane::TerrainLookupTable Plane::Terrain_lookup[] = {
877 : {Mode::Number::FLY_BY_WIRE_B, terrain_bitmask::FLY_BY_WIRE_B},
878 : {Mode::Number::CRUISE, terrain_bitmask::CRUISE},
879 : {Mode::Number::AUTO, terrain_bitmask::AUTO},
880 : {Mode::Number::RTL, terrain_bitmask::RTL},
881 : {Mode::Number::AVOID_ADSB, terrain_bitmask::AVOID_ADSB},
882 : {Mode::Number::GUIDED, terrain_bitmask::GUIDED},
883 : {Mode::Number::LOITER, terrain_bitmask::LOITER},
884 : {Mode::Number::CIRCLE, terrain_bitmask::CIRCLE},
885 : #if HAL_QUADPLANE_ENABLED
886 : {Mode::Number::QRTL, terrain_bitmask::QRTL},
887 : {Mode::Number::QLAND, terrain_bitmask::QLAND},
888 : {Mode::Number::QLOITER, terrain_bitmask::QLOITER},
889 : #endif
890 : #if MODE_AUTOLAND_ENABLED
891 : {Mode::Number::AUTOLAND, terrain_bitmask::AUTOLAND},
892 : #endif
893 : };
894 :
895 32998 : bool Plane::terrain_enabled_in_current_mode() const
896 : {
897 32998 : return terrain_enabled_in_mode(control_mode->mode_number());
898 : }
899 :
900 33010 : bool Plane::terrain_enabled_in_mode(Mode::Number num) const
901 : {
902 : // Global enable
903 33010 : if ((g.terrain_follow.get() & int32_t(terrain_bitmask::ALL)) != 0) {
904 12182 : return true;
905 : }
906 :
907 : // Specific enable
908 94396 : for (const struct TerrainLookupTable entry : Terrain_lookup) {
909 94394 : if (entry.mode_num == num) {
910 20826 : if ((g.terrain_follow.get() & int32_t(entry.bitmask)) != 0) {
911 1 : return true;
912 : }
913 20825 : break;
914 : }
915 : }
916 :
917 20827 : return false;
918 : }
919 : #endif
920 :
921 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
922 : /*
923 : handle a MAV_CMD_SET_HAGL request. The accuracy is ignored
924 : */
925 0 : void Plane::handle_external_hagl(const mavlink_command_int_t &packet)
926 : {
927 0 : auto &hagl = plane.external_hagl;
928 0 : hagl.hagl = packet.param1;
929 0 : hagl.last_update_ms = AP_HAL::millis();
930 0 : hagl.timeout_ms = uint32_t(packet.param3 * 1000);
931 0 : }
932 :
933 : /*
934 : get HAGL from external source if current
935 : */
936 3191008 : bool Plane::get_external_HAGL(float &height_agl)
937 : {
938 3191008 : auto &hagl = plane.external_hagl;
939 3191008 : if (hagl.last_update_ms != 0) {
940 0 : const uint32_t now_ms = AP_HAL::millis();
941 0 : if (now_ms - hagl.last_update_ms <= hagl.timeout_ms) {
942 0 : height_agl = hagl.hagl;
943 0 : return true;
944 : }
945 0 : hagl.last_update_ms = 0;
946 : }
947 3191008 : return false;
948 : }
949 : #endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
950 :
951 : /*
952 : get height for landing. Set using_rangefinder to true if a rangefinder
953 : or external HAGL source is active
954 : */
955 38886 : float Plane::get_landing_height(bool &rangefinder_active)
956 : {
957 : float height;
958 :
959 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
960 : // if external HAGL is active use that
961 38886 : if (get_external_HAGL(height)) {
962 : // ensure no terrain correction is applied - that is the job
963 : // of the external system if it is wanted
964 0 : auto_state.terrain_correction = 0;
965 :
966 : // an external HAGL is considered to be a type of rangefinder
967 0 : rangefinder_active = true;
968 0 : return height;
969 : }
970 : #endif
971 :
972 : // get basic height above target
973 38886 : height = height_above_target();
974 38886 : rangefinder_active = false;
975 :
976 : #if AP_RANGEFINDER_ENABLED
977 : // possibly correct with rangefinder
978 38886 : height -= rangefinder_correction();
979 38886 : rangefinder_active = rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) &&
980 38886 : rangefinder_state.in_use && rangefinder_state.in_range;
981 : #endif
982 :
983 38886 : return height;
984 : }
985 :
986 : /*
987 : if a terrain location doesn't have the relative_alt flag set
988 : then fix the alt and trigger a flow of control error
989 : */
990 8074 : void Plane::fix_terrain_WP(Location &loc, uint32_t linenum)
991 : {
992 8074 : if (loc.terrain_alt && !loc.relative_alt) {
993 0 : AP::internalerror().error(AP_InternalError::error_t::flow_of_control, linenum);
994 : /*
995 : we definitely have a bug, now we need to guess what was
996 : really meant. The lack of the relative_alt flag notionally
997 : means that home.alt has been added to loc.alt, so remove it,
998 : but only if it doesn't lead to a negative terrain altitude
999 : */
1000 0 : if (loc.alt - home.alt > -500) {
1001 0 : loc.alt -= home.alt;
1002 : }
1003 0 : loc.relative_alt = true;
1004 : }
1005 8074 : }
|
