Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Switch into last mode after landing and disarming after RTL/Land #12494

Closed
wants to merge 7 commits into from
Closed

Switch into last mode after landing and disarming after RTL/Land #12494

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
aa7d960
Added logic to transition to manual after auto landing or RTL
Jul 16, 2019
b2e4eab
Keep track of last non-auto state and revert to it
Jul 17, 2019
514c8d9
Removed leftover TODO
Jul 18, 2019
6ff6cf9
Removed landing check
Jul 18, 2019
285dcd7
Added check for recent disarming.
Jul 19, 2019
20ce88c
Removed timing issue
Oct 18, 2019
c945a02
Changed to only switch back to manual mode after RTL, instead of afte…
Oct 25, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,7 @@ static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms;
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sensors to come online for upto 8 seconds */
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms;
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms;
static constexpr uint64_t MISSION_FINISH_DISARM_TIMEOUT = 500_ms;

/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
Expand Down Expand Up @@ -1844,6 +1845,16 @@ Commander::run()
}
}

const bool just_disarmed = !armed.armed && was_armed;
const bool just_finished_rtl = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL
&& _mission_result_sub.get().finished;
const bool last_state_valid = last_non_auto_state != commander_state_s::MAIN_STATE_MAX;

if(just_disarmed && land_detector.landed && just_finished_rtl && last_state_valid){
PX4_INFO("Just finished auto mission, transitioning back to last manual mode.");
julianoes marked this conversation as resolved.
Show resolved Hide resolved
main_state_transition(status, last_non_auto_state, status_flags, &internal_state);
}

/* start geofence result check */
geofence_result_sub.update(&geofence_result);

Expand Down
18 changes: 18 additions & 0 deletions src/modules/commander/state_machine_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,8 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {

static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately

main_state_t last_non_auto_state = commander_state_s::MAIN_STATE_MAX;

void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state);
Expand Down Expand Up @@ -375,6 +377,11 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai

if (ret == TRANSITION_CHANGED) {
if (internal_state->main_state != new_main_state) {
// If transitioning from non-auto to auto state, save the last non-auto state
if (!is_auto_state(internal_state->main_state)) {
last_non_auto_state = internal_state->main_state;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you do two consecutive RTLs this stays in the first non-auto state and will switch back to the manual mode in the first RTL, even if the second RTL was triggered out of an auto state. You need to reset the state in an else statement to not have history in your state machine.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let me make sure I understand correctly: If you have the sequence, for example, Manual -> Mission -> RTL -> Mission -> RTL, then it will switch back to Manual after the final RTL completes. Should it not do this? Under what circumstances exactly should it not switch back to a manual mode after an RTL?

Copy link
Contributor

@julianoes julianoes Oct 25, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess what @LorenzMeier means is that in this case it should switch back to Mission for this case.

This would mean the check should be for any mode that is not RTL, instead of non-auto mode.

@LorenzMeier can you confirm please?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My expectation would be here that it switches back to the mode it had before RTL, not the last manual mode before RTL - otherwise I could have been through a number of mission - RTL - mission - RTL sequences and be left with the manual mode from an hour ago

}

internal_state->main_state = new_main_state;
internal_state->timestamp = hrt_absolute_time();

Expand Down Expand Up @@ -1212,3 +1219,14 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
}
}

bool is_auto_state(main_state_t state)
{
return state == commander_state_s::MAIN_STATE_AUTO_RTL
|| state == commander_state_s::MAIN_STATE_AUTO_LAND
|| state == commander_state_s::MAIN_STATE_AUTO_MISSION
|| state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
|| state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET
|| state == commander_state_s::MAIN_STATE_AUTO_LOITER
|| state == commander_state_s::MAIN_STATE_AUTO_PRECLAND;
}
3 changes: 3 additions & 0 deletions src/modules/commander/state_machine_helper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,4 +147,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
const low_battery_action_t low_bat_action);

extern main_state_t last_non_auto_state;
bool is_auto_state(main_state_t state);

#endif /* STATE_MACHINE_HELPER_H_ */
12 changes: 9 additions & 3 deletions src/modules/navigator/rtl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,9 +95,15 @@ RTL::on_activation()
void
RTL::on_active()
{
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
if (_rtl_state != RTL_STATE_LANDED) {
if (is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
}

} else {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
}
}

Expand Down