From 9ab20b11b6a528ef2ae4197e9cd412de52b1d024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jan 2013 07:42:09 +0100 Subject: Code style --- apps/commander/state_machine_helper.c | 50 ++++++++++++++++++++++++----------- 1 file changed, 35 insertions(+), 15 deletions(-) (limited to 'apps/commander/state_machine_helper.c') diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ff4e6ec00..4c4089f06 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -50,7 +50,7 @@ #include "state_machine_helper.h" -static const char* system_state_txt[] = { +static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", "SYSTEM_STATE_STANDBY", "SYSTEM_STATE_GROUND_READY", @@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); // } else { // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); @@ -120,19 +120,21 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_PREFLIGHT: if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state"); + } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state"); } + break; case SYSTEM_STATE_REBOOT: if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; @@ -140,10 +142,12 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + } else { invalid_state = true; mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } + break; case SYSTEM_STATE_STANDBY: @@ -202,14 +206,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con publish_armed_status(current_status); ret = OK; } + if (invalid_state) { mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition"); ret = ERROR; } + return ret; } -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ /* publish the new state */ current_status->counter++; current_status->timestamp = hrt_absolute_time(); @@ -217,9 +224,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat /* assemble state vector based on flag values */ if (current_status->flag_control_rates_enabled) { current_status->onboard_control_sensors_present |= 0x400; + } else { current_status->onboard_control_sensors_present &= ~0x400; } + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; @@ -235,7 +244,8 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) { +void publish_armed_status(const struct vehicle_status_s *current_status) +{ struct actuator_armed_s armed; armed.armed = current_status->flag_system_armed; /* lock down actuators if required, only in HIL */ @@ -257,7 +267,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_GROUND_ERROR); } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - + // DO NOT abort mission //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); @@ -523,13 +533,14 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c /* set behaviour based on airframe */ if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; + } else { /* assuming a fixed wing, set to direct pass-through */ @@ -556,8 +567,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; current_status->flag_control_manual_enabled = true; + if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { + old_manual_control_mode != current_status->manual_control_mode) { printf("[cmd] att stabilized mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); state_machine_publish(status_pub, current_status, mavlink_fd); @@ -573,6 +585,7 @@ void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *c 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] position guided mode\n"); int old_mode = current_status->flight_mode; @@ -581,6 +594,7 @@ void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *c 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); } @@ -592,7 +606,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur 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("[cmd] auto mode\n"); int old_mode = current_status->flight_mode; @@ -601,6 +615,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur 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); } } @@ -609,10 +624,10 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { uint8_t ret = 1; - + /* Switch on HIL if in standby and not already in HIL mode */ if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { + && !current_status->flag_hil_enabled) { if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { /* Enable HIL on request */ current_status->flag_hil_enabled = true; @@ -620,23 +635,28 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ 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) { + 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 */ if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); } -- cgit v1.2.3