From f8f95078e831ce6351b26b677ce9ea07cc85e9cb Mon Sep 17 00:00:00 2001 From: SungTae Moon Date: Sun, 28 Jan 2018 00:22:29 +0900 Subject: [PATCH] commander status reuse arming_state_names in state_machine_helper (#8667) --- src/modules/commander/commander.cpp | 48 +------------------ .../commander/state_machine_helper.cpp | 6 +-- src/modules/commander/state_machine_helper.h | 2 + 3 files changed, 6 insertions(+), 50 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9a5eb79d6e..9e37bf7259 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -647,55 +647,9 @@ void print_status() warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); warnx("datalink: %s", (status.data_link_lost) ? "LOST" : "OK"); - warnx("main state: %d", internal_state.main_state); warnx("nav state: %d", status.nav_state); - - /* read all relevant states */ - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - struct vehicle_status_s state; - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - - const char *armed_str; - - switch (status.arming_state) { - case vehicle_status_s::ARMING_STATE_INIT: - armed_str = "INIT"; - break; - - case vehicle_status_s::ARMING_STATE_STANDBY: - armed_str = "STANDBY"; - break; - - case vehicle_status_s::ARMING_STATE_ARMED: - armed_str = "ARMED"; - break; - - case vehicle_status_s::ARMING_STATE_ARMED_ERROR: - armed_str = "ARMED_ERROR"; - break; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: - armed_str = "STANDBY_ERROR"; - break; - - case vehicle_status_s::ARMING_STATE_REBOOT: - armed_str = "REBOOT"; - break; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: - armed_str = "IN_AIR_RESTORE"; - break; - - default: - armed_str = "ERR: UNKNOWN STATE"; - break; - } - - px4_close(state_sub); - - - warnx("arming: %s", armed_str); + warnx("arming: %s", arming_state_names[status.arming_state]); } static orb_advert_t status_pub; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5611ec1743..cb36aa2b81 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,7 +93,7 @@ static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle }; // You can index into the array with an arming_state_t in order to get its textual representation -static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = { +const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -374,8 +374,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, if (ret == TRANSITION_DENIED) { /* print to MAVLink and console if we didn't provide any feedback yet */ if (!feedback_provided) { - mavlink_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state], - state_names[new_arming_state]); + mavlink_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", arming_state_names[status->arming_state], + arming_state_names[new_arming_state]); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e1f09d5f78..3112d98924 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -108,6 +108,8 @@ struct status_flags_s { bool gps_failure; // Set to true if a gps failure is detected }; +extern const char *const arming_state_names[]; + bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *status,