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

commander: add desired main state 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/commander_state.msg
View File

@ -18,6 +18,7 @@ uint8 MAIN_STATE_AUTO_PRECLAND = 13
uint8 MAIN_STATE_ORBIT = 14
uint8 MAIN_STATE_MAX = 15
uint8 main_state # main state machine
uint8 main_state
uint8 desired_main_state
uint16 main_state_changes

29
src/modules/commander/Commander.cpp
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,