RTL cleanup - #33868
Open
lthall wants to merge 7 commits into
Open
Conversation
…nge) Rename RTL substages and their handlers to describe the task rather than borrow the Loiter mode name, and to reflect that RTL returns to the nearest rally point or home: RETURN_HOME -> FLY_TO_RETURN_POINT LOITER_AT_HOME -> HOLD_AT_RETURN_POINT loiterathome_start -> hold_at_return_point_start loiterathome_run -> hold_at_return_point_run _loiter_start_time -> _hold_start_time Also corrects stale copy-paste comments. No functional change: enum values, member layout and generated code are unchanged (compiled .text of mode_rtl.cpp.o is byte-identical before and after).
Move the stage transition graph out of run() into advance_state() and route all normal stage changes through set_submode(), which sets _state, clears _state_complete, stamps the stage entry time and runs the stage's entry init. The *_start() functions no longer write the state variables. _hold_start_time becomes _stage_start_ms and is now stamped on every stage entry rather than only on entering HOLD_AT_RETURN_POINT. The hold timer is its only reader and the value at hold entry is unchanged. return_start() now reports failure to set_submode() instead of calling restart_without_terrain() itself. As a result auto_yaw.set_mode_to_default() runs before restart_without_terrain() rather than after; restart does not touch yaw state, so the ordering is inert. The unused in-class initializer for _state is corrected from INITIAL_CLIMB to STARTING. init() always sets _state before any reader runs, so the value is inert; it now matches the true initial stage. The leftover STARTING case in the run() switch, which bumps the state straight to INITIAL_CLIMB, is deliberately retained in this commit to keep it behaviour neutral. It is addressed separately later in this series. No functional change.
Replace the state() and state_complete() accessors with is_complete(), moving ModeAuto::verify_RTL's completion test into ModeRTL. The condition is moved unchanged; it keeps the RTL_ALT_FINAL fix where a non-zero final descent altitude completes without waiting for ground idle. verify_RTL was the only user of the removed accessors. No functional change.
climb_return_run() and hold_at_return_point_run() shared an identical control block: disarm handling, spool state, wp_nav update with terrain failsafe status, vertical position controller and attitude controller. Move it into run_wp_controllers() and keep only the stage completion checks in the callers. The similar looking blocks in Smart RTL are deliberately left alone: they do not set the terrain failsafe status and do not handle the disarmed case, so unifying them would change behaviour. No functional change.
Route all writes of smart_rtl_state through set_submode(), which runs each stage's entry init. The entry init calls now run after the state write rather than before; none of them read smart_rtl_state, so the ordering is inert. The in-class initializer for smart_rtl_state is corrected from PATH_FOLLOW to WAIT_FOR_PATH_CLEANUP. init() always sets the state before any reader runs, so the value is inert; it now matches the true initial stage. Also remove the dead declaration of ModeSmartRTL::land(), which has no definition. No functional change.
return_start() guards against set_wp_destination_loc(return_target) failing, attributing any failure to missing terrain data and requesting a restart from scratch with terrain following disabled by setting the stage back to STARTING with the completion flag set. The per-loop run switch had a leftover line that bumped STARTING straight to INITIAL_CLIMB, erasing the restart request in the same loop before the STARTING stage could rebuild the path. If that branch is taken, the vehicle re-requests the restart every loop and never rebuilds, hanging at the climb point instead of returning home. With the current AC_WPNav this branch is not reachable through terrain data loss: build_path() gives the climb target the same altitude frame as the return target, and set_wp_destination_NED_m() only performs a fallible terrain lookup when a leg changes altitude frame, so the guarded failure could only occur with the EKF origin unavailable. The hang is therefore latent rather than triggerable in normal flight, but the guard exists and the state machine behind it must not dead-end. The other restart source, the terrain failsafe event, arrives between loops and is serviced normally at the top of the next run(); only an in-cycle request from return_start() hit the leftover line. Fix by running the wp controller on the current target for one loop and marking the stage complete so advance_state() rebuilds the path on the next loop. Holding for one loop keeps the change minimal and avoids re-entering advance_state() within a single cycle. No behaviour change is expected in normal flight on current code. If the guarded branch is ever taken, behaviour changes from hanging at the climb point to completing the return with terrain following disabled.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Classification & Testing (check all that apply and add your own)
Description