dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: fix hold after mission logic 

previous change in logic to hold after mission clear also broke rtl, as non-mission takeoff still published a mission result which allowed entering the mission finished condition and always changing state to loiter (ignoring rtl). new logic only switches navigation states if mission is finish and the nav state is explicitly in takeoff state, or in mission state
This commit is contained in:
Thomas Stastny 2022-11-10 15:06:24 +01:00
parent 6dad3a5150
commit 6b9d86680b
1 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/commander/Commander.cpp
View File

@ -1862,17 +1862,22 @@ void Commander::checkForMissionUpdate()
}
}
// handle navigation state auto-switching based on mission_result
if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
&& mission_result.finished) {
if ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
&& (_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
// Transition mode to loiter or auto-mission after takeoff is completed.
if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
} else {
} else {
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
}
} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode.
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
}
}