Line data Source code
1 : #include "Copter.h"
2 :
3 : #if MODE_RTL_ENABLED
4 :
5 : // table of user settable parameters
6 : const AP_Param::GroupInfo ModeRTL::var_info[] = {
7 :
8 : // @Param: ALT_M
9 : // @DisplayName: RTL Altitude
10 : // @Description: The minimum alt above home the vehicle will climb to before returning. If the vehicle is flying higher than this value it will return at its current altitude.
11 : // @Units: m
12 : // @Range: 0.30 3000
13 : // @Increment: 0.1
14 : // @User: Standard
15 : AP_GROUPINFO("ALT_M", 1, ModeRTL, altitude_m, RTL_ALT_M_DEFAULT),
16 :
17 : // @Param: ALT_FINAL_M
18 : // @DisplayName: RTL Final Altitude
19 : // @Description: Altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to zero to land.
20 : // @Units: m
21 : // @Range: 0 10
22 : // @Increment: 0.1
23 : // @User: Standard
24 : AP_GROUPINFO("ALT_FINAL_M", 2, ModeRTL, alt_final_m, RTL_ALT_FINAL_M_DEFAULT),
25 :
26 : // @Param: CLIMB_MIN_M
27 : // @DisplayName: RTL minimum climb
28 : // @Description: The vehicle will climb this many meters during the initial climb portion of the RTL
29 : // @Units: m
30 : // @Range: 0 30
31 : // @Increment: 0.1
32 : // @User: Standard
33 : AP_GROUPINFO("CLIMB_MIN_M", 3, ModeRTL, climb_min_m, RTL_CLIMB_MIN_M_DEFAULT),
34 :
35 : // @Param: SPEED_MS
36 : // @DisplayName: RTL speed
37 : // @Description: The speed in m/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WP_SPD will be used instead.
38 : // @Units: m/s
39 : // @Range: 0 20
40 : // @Increment: 0.5
41 : // @User: Standard
42 : AP_GROUPINFO("SPEED_MS", 4, ModeRTL, speed_ms, 0),
43 :
44 : AP_GROUPEND
45 : };
46 :
47 : // constructor
48 2002 : ModeRTL::ModeRTL() : Mode()
49 : {
50 : // load parameter defaults
51 2002 : AP_Param::setup_object_defaults(this, var_info);
52 2002 : }
53 :
54 : // convert parameters
55 1001 : void ModeRTL::convert_params()
56 : {
57 : // PARAMETER_CONVERSION - Added: Jan-2026 for ArduPilot-4.7
58 :
59 : // return immediately if parameter conversion has already been performed
60 1001 : if (altitude_m.configured() || speed_ms.configured() || alt_final_m.configured() || climb_min_m.configured()) {
61 120 : return;
62 : }
63 :
64 : static const AP_Param::ConversionInfo conversion_info[] = {
65 : { Parameters::k_param_rtl_altitude_cm, 0, AP_PARAM_INT32, "RTL_ALT_M" }, // RTL_ALT moved to RTL_ALT_M
66 : { Parameters::k_param_rtl_speed_cms, 0, AP_PARAM_INT16, "RTL_SPEED_MS" }, // RTL_SPEED moved to RTL_SPEED_MS
67 : { Parameters::k_param_rtl_alt_final_cm, 0, AP_PARAM_INT16, "RTL_ALT_FINAL_M" }, // RTL_ALT_FINAL moved to RTL_ALT_FINAL_M
68 : { Parameters::k_param_rtl_climb_min_cm, 0, AP_PARAM_INT16, "RTL_CLIMB_MIN_M" }, // RTL_CLIMB_MIN moved to RTL_CLIMB_MIN_M
69 : };
70 881 : AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
71 : }
72 :
73 : /*
74 : * Init and run calls for RTL flight mode
75 : *
76 : * There are two parts to RTL, the high level decision making which controls which state we are in
77 : * and the lower implementation of the waypoint or landing controllers within those states
78 : */
79 :
80 : // rtl_init - initialise rtl controller
81 315 : bool ModeRTL::init(bool ignore_checks)
82 : {
83 315 : if (!ignore_checks) {
84 250 : if (!AP::ahrs().home_is_set()) {
85 0 : return false;
86 : }
87 : }
88 : // initialise waypoint and spline controller
89 315 : wp_nav->wp_and_spline_init_m(speed_ms.get());
90 315 : _state = SubMode::STARTING;
91 315 : _state_complete = true; // see run() method below
92 315 : terrain_following_allowed = !copter.failsafe.terrain;
93 : // reset flag indicating if pilot has applied roll or pitch inputs during landing
94 315 : copter.ap.land_repo_active = false;
95 :
96 : // this will be set true if prec land is later active
97 315 : copter.ap.prec_land_active = false;
98 :
99 : #if AC_PRECLAND_ENABLED
100 : // initialise precland state machine
101 315 : copter.precland_statemachine.init();
102 : #endif
103 :
104 315 : return true;
105 : }
106 :
107 : // re-start RTL with terrain following disabled
108 0 : void ModeRTL::restart_without_terrain()
109 : {
110 : #if HAL_LOGGING_ENABLED
111 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
112 : #endif
113 0 : terrain_following_allowed = false;
114 0 : _state = SubMode::STARTING;
115 0 : _state_complete = true;
116 0 : gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
117 0 : }
118 :
119 178406 : ModeRTL::RTLAltType ModeRTL::get_alt_type() const
120 : {
121 : // sanity check parameter
122 178406 : switch ((ModeRTL::RTLAltType)g.rtl_alt_type) {
123 178406 : case RTLAltType::RELATIVE ... RTLAltType::TERRAIN:
124 178406 : return g.rtl_alt_type;
125 : }
126 : // user has an invalid value
127 0 : return RTLAltType::RELATIVE;
128 : }
129 :
130 : // rtl_run - runs the return-to-launch controller
131 : // should be called at 100hz or more
132 5813446 : void ModeRTL::run(bool disarm_on_land)
133 : {
134 5813446 : if (!motors->armed()) {
135 505187 : return;
136 : }
137 :
138 : // check if we need to move to next state
139 5308259 : if (_state_complete) {
140 113094 : switch (_state) {
141 296 : case SubMode::STARTING:
142 296 : build_path();
143 296 : climb_start();
144 296 : break;
145 278 : case SubMode::INITIAL_CLIMB:
146 278 : return_start();
147 278 : break;
148 273 : case SubMode::RETURN_HOME:
149 273 : loiterathome_start();
150 273 : break;
151 273 : case SubMode::LOITER_AT_HOME:
152 273 : if (rtl_path.land || copter.failsafe.radio) {
153 271 : land_start();
154 : } else {
155 2 : descent_start();
156 : }
157 273 : break;
158 8271 : case SubMode::FINAL_DESCENT:
159 : // do nothing
160 8271 : break;
161 103703 : case SubMode::LAND:
162 : // do nothing - rtl_land_run will take care of disarming motors
163 103703 : break;
164 : }
165 : }
166 :
167 : // call the correct run function
168 5308259 : switch (_state) {
169 :
170 0 : case SubMode::STARTING:
171 : // should not be reached:
172 0 : _state = SubMode::INITIAL_CLIMB;
173 : FALLTHROUGH;
174 :
175 1207034 : case SubMode::INITIAL_CLIMB:
176 : case SubMode::RETURN_HOME:
177 1207034 : climb_return_run();
178 1207034 : break;
179 :
180 545130 : case SubMode::LOITER_AT_HOME:
181 545130 : loiterathome_run();
182 545130 : break;
183 :
184 18452 : case SubMode::FINAL_DESCENT:
185 18452 : descent_run();
186 18452 : break;
187 :
188 3537643 : case SubMode::LAND:
189 3537643 : land_run(disarm_on_land);
190 3537643 : break;
191 : }
192 : }
193 :
194 : // rtl_climb_start - initialise climb to RTL altitude
195 296 : void ModeRTL::climb_start()
196 : {
197 296 : _state = SubMode::INITIAL_CLIMB;
198 296 : _state_complete = false;
199 :
200 : // set the destination
201 296 : if (!wp_nav->set_wp_destination_loc(rtl_path.climb_target) || !wp_nav->set_wp_destination_next_loc(rtl_path.return_target)) {
202 : // this should not happen because rtl_build_path will have checked terrain data was available
203 0 : gcs().send_text(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target");
204 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
205 0 : copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE);
206 0 : return;
207 : }
208 :
209 : // hold current yaw during initial climb
210 296 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
211 : }
212 :
213 : // rtl_return_start - initialise return to home
214 278 : void ModeRTL::return_start()
215 : {
216 278 : _state = SubMode::RETURN_HOME;
217 278 : _state_complete = false;
218 :
219 278 : if (!wp_nav->set_wp_destination_loc(rtl_path.return_target)) {
220 : // failure must be caused by missing terrain data, restart RTL
221 0 : restart_without_terrain();
222 : }
223 :
224 : // initialise yaw to point home (maybe)
225 278 : auto_yaw.set_mode_to_default(true);
226 278 : }
227 :
228 : // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
229 : // called by rtl_run at 100hz or more
230 1207034 : void ModeRTL::climb_return_run()
231 : {
232 : // if not armed set throttle to zero and exit immediately
233 1207034 : if (is_disarmed_or_landed()) {
234 0 : make_safe_ground_handling();
235 0 : return;
236 : }
237 :
238 : // set motors to full range
239 1207034 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
240 :
241 : // run waypoint controller
242 1207034 : copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
243 :
244 : // WP_Nav has set the vertical position control targets
245 : // run the vertical position controller and set output throttle
246 1207034 : pos_control->D_update_controller();
247 :
248 : // call attitude controller with auto yaw
249 1207034 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
250 :
251 : // check if we've completed this stage of RTL
252 1207034 : _state_complete = wp_nav->reached_wp_destination();
253 : }
254 :
255 : // loiterathome_start - initialise return to home
256 273 : void ModeRTL::loiterathome_start()
257 : {
258 273 : _state = SubMode::LOITER_AT_HOME;
259 273 : _state_complete = false;
260 273 : _loiter_start_time = millis();
261 :
262 : // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
263 273 : if (auto_yaw.default_mode(true) != AutoYaw::Mode::HOLD) {
264 19 : auto_yaw.set_mode(AutoYaw::Mode::RESET_TO_ARMED_YAW);
265 : } else {
266 254 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
267 : }
268 273 : }
269 :
270 : // rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
271 : // called by rtl_run at 100hz or more
272 545130 : void ModeRTL::loiterathome_run()
273 : {
274 : // if not armed set throttle to zero and exit immediately
275 545130 : if (is_disarmed_or_landed()) {
276 0 : make_safe_ground_handling();
277 0 : return;
278 : }
279 :
280 : // set motors to full range
281 545130 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
282 :
283 : // run waypoint controller
284 545130 : copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
285 :
286 : // WP_Nav has set the vertical position control targets
287 : // run the vertical position controller and set output throttle
288 545130 : pos_control->D_update_controller();
289 :
290 : // call attitude controller with auto yaw
291 545130 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
292 :
293 : // check if we've completed this stage of RTL
294 545130 : if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
295 273 : if (auto_yaw.mode() == AutoYaw::Mode::RESET_TO_ARMED_YAW) {
296 : // check if heading is within 2 degrees of heading when vehicle was armed
297 : // todo: Use the target heading instead of the actual heading to allow landing even if yaw control is lost.
298 19 : if (fabsf(wrap_PI(ahrs.get_yaw_rad() - copter.initial_armed_bearing_rad)) <= radians(2.0)) {
299 19 : _state_complete = true;
300 : }
301 : } else {
302 : // we have loitered long enough
303 254 : _state_complete = true;
304 : }
305 : }
306 : }
307 :
308 : // rtl_descent_start - initialise descent to final alt
309 2 : void ModeRTL::descent_start()
310 : {
311 2 : _state = SubMode::FINAL_DESCENT;
312 2 : _state_complete = false;
313 :
314 : // initialise altitude target to stopping point
315 2 : pos_control->D_init_controller_stopping_point();
316 :
317 : // initialise yaw
318 2 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
319 :
320 : #if AP_LANDINGGEAR_ENABLED
321 : // optionally deploy landing gear
322 2 : copter.landinggear.deploy_for_landing();
323 : #endif
324 2 : }
325 :
326 : // rtl_descent_run - implements the final descent to the RTL_ALT_M
327 : // called by rtl_run at 100hz or more
328 18452 : void ModeRTL::descent_run()
329 : {
330 18452 : Vector2f vel_correction_ms;
331 :
332 : // if not armed set throttle to zero and exit immediately
333 18452 : if (is_disarmed_or_landed()) {
334 0 : make_safe_ground_handling();
335 0 : return;
336 : }
337 :
338 : // process pilot's input
339 18452 : if (rc().has_valid_input()) {
340 18452 : if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
341 0 : LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
342 : // exit land if throttle is high
343 0 : if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
344 : #if MODE_ALTHOLD_ENABLED
345 0 : copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
346 : #endif
347 : }
348 : }
349 :
350 18452 : if (g.land_repositioning) {
351 : // convert pilot input to reposition velocity
352 18452 : vel_correction_ms = get_pilot_desired_velocity(wp_nav->get_wp_acceleration_mss() * 0.5);
353 :
354 : // record if pilot has overridden roll or pitch
355 18452 : if (!vel_correction_ms.is_zero()) {
356 0 : if (!copter.ap.land_repo_active) {
357 0 : LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE);
358 : }
359 0 : copter.ap.land_repo_active = true;
360 : }
361 : }
362 : }
363 :
364 : // set motors to full range
365 18452 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
366 :
367 18452 : Vector2f accel;
368 18452 : pos_control->input_vel_accel_NE_m(vel_correction_ms, accel);
369 18452 : pos_control->NE_update_controller();
370 :
371 : // WP_Nav has set the vertical position control targets
372 : // run the vertical position controller and set output throttle
373 18452 : pos_control->D_set_alt_target_with_slew_m(rtl_path.descent_target.alt * 0.01);
374 18452 : pos_control->D_update_controller();
375 :
376 : // roll & pitch from waypoint controller, yaw rate from pilot
377 18452 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
378 :
379 : // check if we've reached within 20cm of final altitude
380 18452 : _state_complete = fabsf(rtl_path.descent_target.alt * 0.01 - pos_control->get_pos_estimate_U_m()) < 0.2;
381 : }
382 :
383 : // land_start - initialise controllers to loiter over home
384 279 : void ModeRTL::land_start()
385 : {
386 279 : _state = SubMode::LAND;
387 279 : _state_complete = false;
388 :
389 : // set horizontal speed and acceleration limits
390 279 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
391 279 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
392 :
393 : // initialise loiter target destination
394 279 : if (!pos_control->NE_is_active()) {
395 0 : pos_control->NE_init_controller();
396 : }
397 :
398 : // initialise the vertical position controller
399 279 : if (!pos_control->D_is_active()) {
400 0 : pos_control->D_init_controller();
401 : }
402 :
403 : // initialise yaw
404 279 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
405 :
406 : #if AP_LANDINGGEAR_ENABLED
407 : // optionally deploy landing gear
408 279 : copter.landinggear.deploy_for_landing();
409 : #endif
410 279 : }
411 :
412 1974602 : bool ModeRTL::is_landing() const
413 : {
414 1974602 : return _state == SubMode::LAND;
415 : }
416 :
417 : // land_run - run the landing controllers to put the aircraft on the ground
418 : // called by rtl_run at 100hz or more
419 3566207 : void ModeRTL::land_run(bool disarm_on_land)
420 : {
421 : // check if we've completed this stage of RTL
422 3566207 : _state_complete = copter.ap.land_complete;
423 :
424 : // disarm when the landing detector says we've landed
425 3566207 : if (disarm_on_land && copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
426 229 : copter.arming.disarm(AP_Arming::Method::LANDED);
427 : }
428 :
429 : // if not armed set throttle to zero and exit immediately
430 3566207 : if (is_disarmed_or_landed()) {
431 113171 : make_safe_ground_handling();
432 113171 : return;
433 : }
434 :
435 : // set motors to full range
436 3453036 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
437 :
438 : // run normal landing or precision landing (if enabled)
439 3453036 : land_run_normal_or_precland();
440 : }
441 :
442 296 : void ModeRTL::build_path()
443 : {
444 : // origin point is our stopping point
445 296 : rtl_path.origin_point = get_stopping_point();
446 296 : rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
447 :
448 : // compute return target
449 296 : compute_return_target();
450 :
451 : // climb target is above our origin point at the return altitude
452 296 : rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
453 :
454 : // descent target is below return target at rtl_alt_final_m
455 296 : rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, alt_final_m.get() * 100, Location::AltFrame::ABOVE_HOME);
456 :
457 : // Target altitude is passed directly to the position controller so must be relative to origin
458 296 : rtl_path.descent_target.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
459 :
460 : // set land flag
461 296 : rtl_path.land = alt_final_m.get() <= 0;
462 296 : }
463 :
464 : // compute the return target - home or rally point
465 : // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
466 296 : void ModeRTL::compute_return_target()
467 : {
468 : // set return target to nearest rally point or home position
469 : #if HAL_RALLY_ENABLED
470 296 : rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
471 296 : rtl_path.return_target.change_alt_frame(Location::AltFrame::ABSOLUTE);
472 : #else
473 : rtl_path.return_target = ahrs.get_home();
474 : #endif
475 :
476 : // get position controller Z-axis offset in cm above EKF origin
477 296 : float pos_offset_u_m = pos_control->get_pos_offset_U_m();
478 :
479 : // curr_alt_m is current altitude, with any offset removed, above home or above terrain depending upon use_terrain
480 296 : float curr_alt_m = copter.current_loc.alt * 0.01 - pos_offset_u_m;
481 :
482 : // determine altitude type of return journey (alt-above-home, alt-above-terrain using range finder or alt-above-terrain using terrain database)
483 296 : ReturnTargetAltType alt_type = ReturnTargetAltType::RELATIVE;
484 296 : if (terrain_following_allowed && (get_alt_type() == RTLAltType::TERRAIN)) {
485 : // convert RTL_ALT_TYPE and WP_RFNG_USE parameters to ReturnTargetAltType
486 0 : switch (wp_nav->get_terrain_source()) {
487 0 : case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE:
488 0 : alt_type = ReturnTargetAltType::RELATIVE;
489 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND);
490 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home");
491 0 : break;
492 0 : case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
493 0 : alt_type = ReturnTargetAltType::RANGEFINDER;
494 0 : break;
495 0 : case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
496 0 : alt_type = ReturnTargetAltType::TERRAINDATABASE;
497 0 : break;
498 : }
499 : }
500 :
501 : // set curr_alt_m and return_target.alt from range finder
502 296 : if (alt_type == ReturnTargetAltType::RANGEFINDER) {
503 0 : if (copter.get_rangefinder_height_interpolated_m(curr_alt_m)) {
504 : // subtract vertical offset from altitude.
505 0 : curr_alt_m -= pos_offset_u_m;
506 : // set return_target.alt
507 0 : rtl_path.return_target.set_alt_m(MAX(curr_alt_m + MAX(0.0f, climb_min_m.get()), MAX(altitude_m.get(), RTL_ALT_MIN_M)), Location::AltFrame::ABOVE_TERRAIN);
508 : } else {
509 : // fallback to relative alt and warn user
510 0 : alt_type = ReturnTargetAltType::RELATIVE;
511 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: rangefinder unhealthy, using alt-above-home");
512 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND);
513 : }
514 : }
515 :
516 : // set curr_alt_m and return_target.alt from terrain database
517 296 : if (alt_type == ReturnTargetAltType::TERRAINDATABASE) {
518 : // set curr_alt_m to current altitude above terrain
519 : // convert return_target.alt from an abs (above MSL) to altitude above terrain
520 : // Note: the return_target may be a rally point with the alt set above the terrain alt (like the top of a building)
521 : float curr_terr_alt_m;
522 0 : if (copter.current_loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, curr_terr_alt_m) &&
523 0 : rtl_path.return_target.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
524 : // subtract vertical offset from altitude.
525 0 : curr_alt_m = curr_terr_alt_m - pos_offset_u_m;
526 : } else {
527 : // fallback to relative alt and warn user
528 0 : alt_type = ReturnTargetAltType::RELATIVE;
529 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
530 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home");
531 : }
532 : }
533 :
534 : // for the default case we must convert return-target alt (which is an absolute alt) to alt-above-home
535 296 : if (alt_type == ReturnTargetAltType::RELATIVE) {
536 296 : if (!rtl_path.return_target.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
537 : // this should never happen but just in case
538 0 : rtl_path.return_target.set_alt_m(0, Location::AltFrame::ABOVE_HOME);
539 0 : gcs().send_text(MAV_SEVERITY_WARNING, "RTL: unexpected error calculating target alt");
540 : }
541 : }
542 :
543 : // set new target altitude to return target altitude
544 : // Note: this is alt-above-home or terrain-alt depending upon rtl_alt_type
545 : // Note: ignore negative altitudes which could happen if user enters negative altitude for rally point or terrain is higher at rally point compared to home
546 296 : float target_alt_m = MAX(rtl_path.return_target.alt, 0) * 0.01;
547 :
548 : // increase target to maximum of current altitude + climb_min and rtl altitude
549 296 : const float min_rtl_alt_m = MAX(RTL_ALT_MIN_M, curr_alt_m + MAX(0.0f, climb_min_m.get()));
550 296 : target_alt_m = MAX(target_alt_m, MAX(altitude_m.get(), min_rtl_alt_m));
551 :
552 : // reduce climb if close to return target
553 296 : float rtl_return_dist_m = rtl_path.return_target.get_distance(rtl_path.origin_point);
554 : // don't allow really shallow slopes
555 296 : if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
556 296 : target_alt_m = MIN(target_alt_m, MAX(rtl_return_dist_m * g.rtl_cone_slope, min_rtl_alt_m));
557 : }
558 :
559 : // set returned target alt to new target_alt_m (don't change altitude type)
560 296 : rtl_path.return_target.set_alt_m(target_alt_m, (alt_type == ReturnTargetAltType::RELATIVE) ? Location::AltFrame::ABOVE_HOME : Location::AltFrame::ABOVE_TERRAIN);
561 :
562 : #if AP_FENCE_ENABLED
563 : // ensure not above fence altitude if alt fence is enabled
564 : // Note: because the rtl_path.climb_target's altitude is simply copied from the return_target's altitude,
565 : // if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
566 : // the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better
567 : // to apply the fence alt limit independently on the origin_point and return_target
568 296 : if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
569 : // get return target in max alt frame so it can be compared to fence's alt
570 16 : if (rtl_path.return_target.get_alt_m(copter.fence.get_alt_max_frame(), target_alt_m)) {
571 16 : float fence_alt_m = copter.fence.get_safe_alt_max_m();
572 16 : if (target_alt_m > fence_alt_m) {
573 : // reduce target alt to the fence alt
574 13 : rtl_path.return_target.alt -= (target_alt_m - fence_alt_m) * 100.0;
575 : }
576 : }
577 : }
578 : #endif
579 :
580 : // ensure we do not descend
581 296 : rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt_m * 100.0);
582 296 : }
583 :
584 77195 : bool ModeRTL::get_wp(Location& destination) const
585 : {
586 : // provide target in states which use wp_nav
587 77195 : switch (_state) {
588 24933 : case SubMode::STARTING:
589 : case SubMode::INITIAL_CLIMB:
590 : case SubMode::RETURN_HOME:
591 : case SubMode::LOITER_AT_HOME:
592 : case SubMode::FINAL_DESCENT:
593 24933 : return wp_nav->get_oa_wp_destination(destination);
594 52262 : case SubMode::LAND:
595 52262 : return false;
596 : }
597 :
598 : // we should never get here but just in case
599 0 : return false;
600 : }
601 :
602 71706 : float ModeRTL::wp_distance_m() const
603 : {
604 71706 : return wp_nav->get_wp_distance_to_destination_m();
605 : }
606 :
607 71706 : float ModeRTL::wp_bearing_deg() const
608 : {
609 71706 : return degrees(wp_nav->get_wp_bearing_to_destination_rad());
610 : }
611 :
612 : // returns true if pilot's yaw input should be used to adjust vehicle's heading
613 5060639 : bool ModeRTL::use_pilot_yaw(void) const
614 : {
615 : // use land mode setting during descent
616 5060639 : if (_state == SubMode::FINAL_DESCENT || _state == SubMode::LAND) {
617 3348658 : return copter.mode_land.use_pilot_yaw();
618 : }
619 1711981 : return !option_is_enabled(Option::IgnorePilotYaw);
620 : }
621 :
622 397 : bool ModeRTL::set_speed_NE_ms(float speed_ne_ms)
623 : {
624 397 : copter.wp_nav->set_speed_NE_ms(speed_ne_ms);
625 397 : return true;
626 : }
627 :
628 0 : bool ModeRTL::set_speed_up_ms(float speed_up_ms)
629 : {
630 0 : copter.wp_nav->set_speed_up_ms(speed_up_ms);
631 0 : return true;
632 : }
633 :
634 0 : bool ModeRTL::set_speed_down_ms(float speed_down_ms)
635 : {
636 0 : copter.wp_nav->set_speed_down_ms(speed_down_ms);
637 0 : return true;
638 : }
639 :
640 1711981 : bool ModeRTL::option_is_enabled(Option option) const
641 : {
642 1711981 : return ((copter.g2.rtl_options & (uint32_t)option) != 0);
643 : }
644 :
645 : #endif
|
