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

commander: take previous main state out of status

sbg
Julian Oes 9 years ago
parent
181eb49da8
commit
141b984d5b
4 changed files with 51 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      msg/vehicle_status.msg
  2. 94
      src/modules/commander/commander.cpp
  3. 4
      src/modules/commander/state_machine_helper.cpp
  4. 2
      src/modules/commander/state_machine_helper.h

1
msg/vehicle_status.msg
Unescape View File

@ -60,7 +60,6 @@ uint8 RC_IN_MODE_GENERATED = 2 @@ -60,7 +60,6 @@ uint8 RC_IN_MODE_GENERATED = 2
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
uint8 main_state # main state machine
uint8 main_state_prev # previous main state
uint8 nav_state # set navigation state machine to specified value
uint8 arming_state # current arming state
uint8 hil_state # current hil state

94
src/modules/commander/commander.cpp
Unescape View File

@ -227,6 +227,8 @@ static bool cb_usb; @@ -227,6 +227,8 @@ static bool cb_usb;
static bool calibration_enabled = false;
static uint8_t main_state_prev = 0;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@ -704,34 +706,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -704,34 +706,34 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
if (custom_sub_mode > 0) {
switch(custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev);
break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev);
break;
case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET:
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET);
@ -744,12 +746,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -744,12 +746,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
} else {
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
/* RATTITUDE */
@ -757,30 +759,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -757,30 +759,30 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else {
/* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
}
}
}
@ -992,7 +994,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -992,7 +994,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@ -1006,7 +1008,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -1006,7 +1008,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else {
/* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard);
res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev);
status_local->offboard_control_set_by_command = false;
}
}
@ -1014,7 +1016,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -1014,7 +1016,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev)) {
warnx("taking off!");
} else {
warnx("takeoff denied");
@ -1025,7 +1027,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s @@ -1025,7 +1027,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND, main_state_prev)) {
warnx("landing!");
} else {
warnx("landing denied");
@ -1234,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1234,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[])
status.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
@ -2176,13 +2178,13 @@ int commander_thread_main(int argc, char *argv[]) @@ -2176,13 +2178,13 @@ int commander_thread_main(int argc, char *argv[])
break;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev)) {
geofence_loiter_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) {
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev)) {
geofence_rtl_on = true;
}
break;
@ -2229,7 +2231,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -2229,7 +2231,7 @@ int commander_thread_main(int argc, char *argv[])
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, geofence_main_state_before_violation);
main_state_transition(&status, geofence_main_state_before_violation, main_state_prev);
}
}
}
@ -2541,15 +2543,15 @@ int commander_thread_main(int argc, char *argv[]) @@ -2541,15 +2543,15 @@ int commander_thread_main(int argc, char *argv[])
/* reset main state after takeoff or land has been completed */
/* only switch back to at least altitude controlled modes */
if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL ||
status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) {
if (main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL ||
main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) {
if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.condition_landed)) {
main_state_transition(&status, status.main_state_prev);
main_state_transition(&status, main_state_prev, main_state_prev);
}
}
@ -2913,7 +2915,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -2913,7 +2915,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
// just delete this and respond to mode switches
/* if offboard is set already by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
}
/* manual setpoint has not updated, do not re-evaluate it */
@ -2942,7 +2944,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -2942,7 +2944,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD, main_state_prev);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@ -2957,13 +2959,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -2957,13 +2959,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* RTL switch overrides main switch */
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
warnx("RTL switch changed and ON!");
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL, main_state_prev);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO RTL");
/* fallback to LOITER if home position not set */
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
}
if (res != TRANSITION_DENIED) {
@ -3076,11 +3078,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3076,11 +3078,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
*/
// XXX: put ACRO and STAB on separate switches
if (status.is_rotary_wing && !status.is_vtol) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO, main_state_prev);
} else if (!status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
}
}
@ -3088,12 +3090,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3088,12 +3090,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE, main_state_prev);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB, main_state_prev);
}
}else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
}
// TRANSITION_DENIED is not possible here
@ -3101,7 +3103,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3101,7 +3103,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -3111,7 +3113,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3111,7 +3113,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
@ -3122,13 +3124,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3122,13 +3124,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
// TRANSITION_DENIED is not possible here
break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -3137,7 +3139,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3137,7 +3139,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO PAUSE");
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -3146,7 +3148,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3146,7 +3148,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@ -3154,21 +3156,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s @@ -3154,21 +3156,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to POSCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL, main_state_prev);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL, main_state_prev);
// TRANSITION_DENIED is not possible here
break;

4
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -339,7 +339,7 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet @@ -339,7 +339,7 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev)
{
transition_result_t ret = TRANSITION_DENIED;
@ -403,7 +403,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta @@ -403,7 +403,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state_prev = status->main_state;
main_state_prev = status->main_state;
status->main_state = new_main_state;
} else {
ret = TRANSITION_NOT_CHANGED;

2
src/modules/commander/state_machine_helper.h
Unescape View File

@ -67,7 +67,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta @@ -67,7 +67,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta
bool circuit_breaker_engaged_power_check,
bool cb_usb);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);

Write Preview
Loading…
Cancel
Save