From 043ad3c33e923c0f4970bf2d75585da733fdfe2c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Dec 2017 19:51:36 -0500 Subject: [PATCH] commander vehicle_status_flags only publish if changed --- src/modules/commander/commander.cpp | 30 +++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2a10527b29..863bed9b0a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -345,7 +345,7 @@ static void answer_command(struct vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); /* publish vehicle status flags from the global variable status_flags*/ -static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub); +static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status_flags_s& vehicle_status_flags); static int power_button_state_notification_cb(board_power_button_state_notification_e request) @@ -1543,6 +1543,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t commander_state_pub = nullptr; + vehicle_status_flags_s vehicle_status_flags = {}; orb_advert_t vehicle_status_flags_pub = nullptr; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { @@ -3324,7 +3325,7 @@ int commander_thread_main(int argc, char *argv[]) } /* publish vehicle_status_flags */ - publish_status_flags(vehicle_status_flags_pub); + publish_status_flags(vehicle_status_flags_pub, vehicle_status_flags); /* publish internal state for logging purposes */ if (commander_state_pub != nullptr) { @@ -4571,9 +4572,10 @@ void *commander_low_prio_loop(void *arg) return nullptr; } -void publish_status_flags(orb_advert_t &vehicle_status_flags_pub) { - struct vehicle_status_flags_s v_flags; - memset(&v_flags, 0, sizeof(v_flags)); +void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status_flags_s& vehicle_status_flags) { + + vehicle_status_flags_s v_flags = {}; + /* set condition status flags */ if (status_flags.condition_calibration_enabled) { v_flags.conditions |= vehicle_status_flags_s::CONDITION_CALIBRATION_ENABLE_MASK; @@ -4682,10 +4684,18 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub) { v_flags.other_flags |= vehicle_status_flags_s::EVER_HAD_BAROMETER_DATA_MASK; } - /* publish vehicle_status_flags */ - if (vehicle_status_flags_pub != nullptr) { - orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &v_flags); - } else { - vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &v_flags); + if ((v_flags.conditions != vehicle_status_flags.conditions) || + (v_flags.other_flags != vehicle_status_flags.other_flags) || + (v_flags.circuit_breakers != vehicle_status_flags.circuit_breakers)) { + + /* publish vehicle_status_flags */ + vehicle_status_flags = v_flags; + vehicle_status_flags.timestamp = hrt_absolute_time(); + + if (vehicle_status_flags_pub != nullptr) { + orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &vehicle_status_flags); + } else { + vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &vehicle_status_flags); + } } }