|
|
|
@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
@@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|
|
|
|
/* MANUAL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_MANUAL); |
|
|
|
|
|
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { |
|
|
|
|
/* ALTCTRL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); |
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { |
|
|
|
|
/* ALTCTL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_ALTCTL); |
|
|
|
|
|
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { |
|
|
|
|
/* POSCTRL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_POSCTRL); |
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { |
|
|
|
|
/* POSCTL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_POSCTL); |
|
|
|
|
|
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { |
|
|
|
|
/* AUTO */ |
|
|
|
@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
@@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|
|
|
|
|
|
|
|
|
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { |
|
|
|
|
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { |
|
|
|
|
/* POSCTRL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_POSCTRL); |
|
|
|
|
/* POSCTL */ |
|
|
|
|
main_res = main_state_transition(status, MAIN_STATE_POSCTL); |
|
|
|
|
|
|
|
|
|
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { |
|
|
|
|
/* MANUAL */ |
|
|
|
@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[])
@@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[])
|
|
|
|
|
|
|
|
|
|
char *main_states_str[MAIN_STATE_MAX]; |
|
|
|
|
main_states_str[0] = "MANUAL"; |
|
|
|
|
main_states_str[1] = "ALTCTRL"; |
|
|
|
|
main_states_str[2] = "POSCTRL"; |
|
|
|
|
main_states_str[1] = "ALTCTL"; |
|
|
|
|
main_states_str[2] = "POSCTL"; |
|
|
|
|
main_states_str[3] = "AUTO"; |
|
|
|
|
|
|
|
|
|
char *arming_states_str[ARMING_STATE_MAX]; |
|
|
|
@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[])
@@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[])
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// TODO remove this hack
|
|
|
|
|
/* flight termination in manual mode if assist switch is on posctrl position */ |
|
|
|
|
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { |
|
|
|
|
/* flight termination in manual mode if assist switch is on POSCTL position */ |
|
|
|
|
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { |
|
|
|
|
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { |
|
|
|
|
tune_positive(armed.armed); |
|
|
|
|
} |
|
|
|
@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
@@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case SWITCH_POS_MIDDLE: // ASSIST
|
|
|
|
|
if (sp_man->posctrl_switch == SWITCH_POS_ON) { |
|
|
|
|
res = main_state_transition(status, MAIN_STATE_POSCTRL); |
|
|
|
|
if (sp_man->posctl_switch == SWITCH_POS_ON) { |
|
|
|
|
res = main_state_transition(status, MAIN_STATE_POSCTL); |
|
|
|
|
|
|
|
|
|
if (res != TRANSITION_DENIED) { |
|
|
|
|
break; // changed successfully or already in this state
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// else fallback to ALTCTRL
|
|
|
|
|
print_reject_mode(status, "POSCTRL"); |
|
|
|
|
// else fallback to ALTCTL
|
|
|
|
|
print_reject_mode(status, "POSCTL"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
res = main_state_transition(status, MAIN_STATE_ALTCTRL); |
|
|
|
|
res = main_state_transition(status, MAIN_STATE_ALTCTL); |
|
|
|
|
|
|
|
|
|
if (res != TRANSITION_DENIED) { |
|
|
|
|
break; // changed successfully or already in this mode
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (sp_man->posctrl_switch != SWITCH_POS_ON) { |
|
|
|
|
print_reject_mode(status, "ALTCTRL"); |
|
|
|
|
if (sp_man->posctl_switch != SWITCH_POS_ON) { |
|
|
|
|
print_reject_mode(status, "ALTCTL"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// else fallback to MANUAL
|
|
|
|
@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
@@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
|
|
|
|
break; // changed successfully or already in this state
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// else fallback to ALTCTRL (POSCTRL likely will not work too)
|
|
|
|
|
// else fallback to ALTCTL (POSCTL likely will not work too)
|
|
|
|
|
print_reject_mode(status, "AUTO"); |
|
|
|
|
res = main_state_transition(status, MAIN_STATE_ALTCTRL); |
|
|
|
|
res = main_state_transition(status, MAIN_STATE_ALTCTL); |
|
|
|
|
|
|
|
|
|
if (res != TRANSITION_DENIED) { |
|
|
|
|
break; // changed successfully or already in this state
|
|
|
|
@ -1674,7 +1674,7 @@ set_control_mode()
@@ -1674,7 +1674,7 @@ set_control_mode()
|
|
|
|
|
control_mode.flag_control_velocity_enabled = false; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAIN_STATE_ALTCTRL: |
|
|
|
|
case MAIN_STATE_ALTCTL: |
|
|
|
|
control_mode.flag_control_manual_enabled = true; |
|
|
|
|
control_mode.flag_control_auto_enabled = false; |
|
|
|
|
control_mode.flag_control_rates_enabled = true; |
|
|
|
@ -1685,7 +1685,7 @@ set_control_mode()
@@ -1685,7 +1685,7 @@ set_control_mode()
|
|
|
|
|
control_mode.flag_control_velocity_enabled = false; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAIN_STATE_POSCTRL: |
|
|
|
|
case MAIN_STATE_POSCTL: |
|
|
|
|
control_mode.flag_control_manual_enabled = true; |
|
|
|
|
control_mode.flag_control_auto_enabled = false; |
|
|
|
|
control_mode.flag_control_rates_enabled = true; |
|
|
|
|