fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander vehicle_status_flags only publish if changed

Browse Source
This commit is contained in:
Daniel Agar
2017-12-22 10:42:14 -05:00
parent 176738c688
commit 043ad3c33e
1 changed files with 20 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+20 -10
View File
@@ -345,7 +345,7 @@ static void answer_command(struct vehicle_command_s &cmd, unsigned result,
orb_advert_t &command_ack_pub); orb_advert_t &command_ack_pub);
/* publish vehicle status flags from the global variable status_flags*/ /* 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) 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; orb_advert_t commander_state_pub = nullptr;
vehicle_status_flags_s vehicle_status_flags = {};
orb_advert_t vehicle_status_flags_pub = nullptr; orb_advert_t vehicle_status_flags_pub = nullptr;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { 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 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 */ /* publish internal state for logging purposes */
if (commander_state_pub != nullptr) { if (commander_state_pub != nullptr) {
@@ -4571,9 +4572,10 @@ void *commander_low_prio_loop(void *arg)
return nullptr; return nullptr;
} }
void publish_status_flags(orb_advert_t &vehicle_status_flags_pub) { void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status_flags_s& vehicle_status_flags) {
struct vehicle_status_flags_s v_flags;
memset(&v_flags, 0, sizeof(v_flags)); vehicle_status_flags_s v_flags = {};
/* set condition status flags */ /* set condition status flags */
if (status_flags.condition_calibration_enabled) { if (status_flags.condition_calibration_enabled) {
v_flags.conditions |= vehicle_status_flags_s::CONDITION_CALIBRATION_ENABLE_MASK; 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; v_flags.other_flags |= vehicle_status_flags_s::EVER_HAD_BAROMETER_DATA_MASK;
} }
/* publish vehicle_status_flags */ if ((v_flags.conditions != vehicle_status_flags.conditions) ||
if (vehicle_status_flags_pub != nullptr) { (v_flags.other_flags != vehicle_status_flags.other_flags) ||
orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &v_flags); (v_flags.circuit_breakers != vehicle_status_flags.circuit_breakers)) {
} else {
vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &v_flags); /* 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);
}
} }
} }