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

commander consolidate periodic state publishing

Browse Source
This commit is contained in:
Daniel Agar
2017-12-20 20:04:30 -05:00
parent 1ea5de43cf
commit 5d6edcc15d
1 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+12 -12
View File
@@ -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);