fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-20 19:51:36 -05:00
parent 176738c688
commit 043ad3c33e
1 changed files with 20 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/commander/commander.cpp
View File

@ -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);
}
}
}