zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: move main_state_transition out 

This is a first step to having a desired main state.
master
Julian Oes 4 years ago committed by Matthias Grob
parent
e2e908d0fc
commit
66b069e788
1 changed files with 23 additions and 31 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 54
      src/modules/commander/Commander.cpp

54
src/modules/commander/Commander.cpp
Unescape View File

@ -788,36 +788,33 @@ Commander::handle_command(const vehicle_command_s &cmd)
uint8_t custom_main_mode = (uint8_t)cmd.param2; uint8_t custom_main_mode = (uint8_t)cmd.param2;
uint8_t custom_sub_mode = (uint8_t)cmd.param3; uint8_t custom_sub_mode = (uint8_t)cmd.param3;
uint8_t desired_main_state = commander_state_s::MAIN_STATE_MAX;
transition_result_t main_ret = TRANSITION_NOT_CHANGED; transition_result_t main_ret = TRANSITION_NOT_CHANGED;
if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */ desired_main_state = commander_state_s::MAIN_STATE_MANUAL;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */ desired_main_state = commander_state_s::MAIN_STATE_ALTCTL;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
reset_posvel_validity(); reset_posvel_validity();
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
if (custom_sub_mode > 0) { if (custom_sub_mode > 0) {
reset_posvel_validity(); reset_posvel_validity();
switch (custom_sub_mode) { switch (custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
if (_status_flags.condition_auto_mission_available) { if (_status_flags.condition_auto_mission_available) {
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
} else { } else {
main_ret = TRANSITION_DENIED; main_ret = TRANSITION_DENIED;
@ -826,24 +823,23 @@ Commander::handle_command(const vehicle_command_s &cmd)
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL: case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND: case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, _status_flags, desired_main_state = commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET;
_internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
break; break;
default: default:
@ -855,46 +851,42 @@ Commander::handle_command(const vehicle_command_s &cmd)
} }
} else { } else {
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
} }
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */ desired_main_state = commander_state_s::MAIN_STATE_ACRO;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */ desired_main_state = commander_state_s::MAIN_STATE_STAB;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
reset_posvel_validity(); reset_posvel_validity();
desired_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
/* OFFBOARD */
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state);
} }
} else { } else {
/* use base mode */ /* use base mode */
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */ desired_main_state = commander_state_s::MAIN_STATE_STAB;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else { } else {
/* MANUAL */ desired_main_state = commander_state_s::MAIN_STATE_MANUAL;
main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} }
} }
} }
if (desired_main_state != commander_state_s::MAIN_STATE_MAX) {
main_ret = main_state_transition(_status, desired_main_state, _status_flags, _internal_state);
}
if (main_ret != TRANSITION_DENIED) { if (main_ret != TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

Write Preview
Loading…
Cancel
Save