From 5d6edcc15d928000e166b981b8a849f7eb5ff5a0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Dec 2017 20:04:30 -0500 Subject: [PATCH] commander consolidate periodic state publishing --- src/modules/commander/commander.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 863bed9b0a..04a8ed9fc7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3225,7 +3225,7 @@ int commander_thread_main(int argc, char *argv[]) main_state_changed = false; } - /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); control_mode.timestamp = now; @@ -3249,6 +3249,17 @@ int commander_thread_main(int argc, char *argv[]) armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000); } orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + + /* publish internal state for logging purposes */ + if (commander_state_pub != nullptr) { + orb_publish(ORB_ID(commander_state), commander_state_pub, &internal_state); + + } else { + commander_state_pub = orb_advertise(ORB_ID(commander_state), &internal_state); + } + + /* publish vehicle_status_flags */ + publish_status_flags(vehicle_status_flags_pub, vehicle_status_flags); } /* play arming and battery warning tunes */ @@ -3324,17 +3335,6 @@ int commander_thread_main(int argc, char *argv[]) have_taken_off_since_arming = false; } - /* publish vehicle_status_flags */ - publish_status_flags(vehicle_status_flags_pub, vehicle_status_flags); - - /* publish internal state for logging purposes */ - if (commander_state_pub != nullptr) { - orb_publish(ORB_ID(commander_state), commander_state_pub, &internal_state); - - } else { - commander_state_pub = orb_advertise(ORB_ID(commander_state), &internal_state); - } - arm_auth_update(now); usleep(COMMANDER_MONITORING_INTERVAL);