diff options
Diffstat (limited to 'apps/commander/state_machine_helper.c')
-rw-r--r-- | apps/commander/state_machine_helper.c | 145 |
1 files changed, 91 insertions, 54 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aa916dafa..a8cef8c01 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); + fprintf(stderr, "[cmd] EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: @@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); + fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: @@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* prevent actuators from arming */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); + fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system"); break; case SYSTEM_STATE_PREFLIGHT: @@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state"); } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state"); } break; @@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); + mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } break; @@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* standby enforces disarmed */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: @@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: @@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: @@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: @@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode"); break; default: @@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con ret = OK; } if (invalid_state) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition"); ret = ERROR; } return ret; @@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } void publish_armed_status(const struct vehicle_status_s *current_status) { @@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { */ void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); + fprintf(stderr, "[cmd] EMERGENCY HANDLER\n"); /* Depending on the current state go to one of the error states */ if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { @@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); } else { - fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine); } } @@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); } else { - //global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); } } @@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[commander] arming\n"); + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } } @@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[commander] going standby\n"); + printf("[cmd] going standby\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] MISSION ABORT!\n"); + printf("[cmd] MISSION ABORT!\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } } @@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; @@ -525,58 +526,92 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] manual mode\n"); + printf("[cmd] manual mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); } } void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE"); + tune_error(); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + } +} +void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. HOLD MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] stabilized mode\n"); + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[commander] auto mode\n"); + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { - printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !current_status->flag_hil_enabled) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.") + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!") + } else { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.") + } } /* switch manual / auto */ @@ -595,7 +630,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; - printf("[commander] arming due to command request\n"); + printf("[cmd] arming due to command request\n"); } } @@ -605,13 +640,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; - printf("[commander] disarming due to command request\n"); + printf("[cmd] disarming due to command request\n"); } } /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL"); ret = ERROR; } @@ -636,7 +672,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { printf("system will reboot\n"); - //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + mavlink_log_critical(mavlink_fd, "[cmd] Rebooting.."); + usleep(200000); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); ret = 0; } |