fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 06:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: add desired main state

Browse Source 
This is an intermediate solution to carry forward the initial state of
the mode slot. Basically, it allows that we start up in Stabilized but
switch to POSCTL as soon we have the required GPS.
This commit is contained in:
Julian Oes
2021-05-06 15:27:51 +02:00
committed by Matthias Grob
parent b6af068f25
commit e49b596edc
2 changed files with 31 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+29
View File
@@ -733,6 +733,8 @@ Commander::Commander() :
// default for vtol is rotary wing
_vtol_status.vtol_in_rw_mode = true;
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
mission_init();
}
@@ -879,6 +881,17 @@ Commander::handle_command(const vehicle_command_s &cmd)
reset_posvel_validity();
main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state);
// If the command is internal (e.g. sent by RC), and we were not (yet) able to switch
// to it, we will try again later. However, we only do that for ALTCTL and POSCTL.
if (!cmd.from_external && main_ret == TRANSITION_DENIED &&
(desired_main_state == commander_state_s::MAIN_STATE_ALTCTL ||
desired_main_state == commander_state_s::MAIN_STATE_POSCTL)) {
_internal_state.desired_main_state = desired_main_state;
} else {
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
}
}
if (main_ret != TRANSITION_DENIED) {
@@ -2710,6 +2723,22 @@ Commander::run()
_imbalanced_propeller_check_triggered = false;
}
// If we have an desired state, we should try to reach it but only when still disarmed.
if (_internal_state.desired_main_state != commander_state_s::commander_state_s::MAIN_STATE_MAX &&
!_armed.armed) {
const transition_result_t desired_ret = main_state_transition(_status, _internal_state.desired_main_state,
_status_flags, _internal_state);
if (desired_ret == TRANSITION_CHANGED) {
// Reset it for next time.
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
}
} else if (_armed.armed) {
// Once armed we reset it.
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;
}
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(_status,
_armed,