fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 21:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #446 from PX4/commander_fixes

Browse Source 
commander fixes
This commit is contained in:
Lorenz Meier
2013-10-13 03:10:23 -07:00
parent 90127b2d2a 3fd2048188
commit db4ff7f080
3 changed files with 6 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+6 -18
View File
@@ -1193,31 +1193,19 @@ int commander_thread_main(int argc, char *argv[])
bool main_state_changed = check_main_state_changed();
bool navigation_state_changed = check_navigation_state_changed();
hrt_abstime t1 = hrt_absolute_time();
if (navigation_state_changed || arming_state_changed) {
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
}
if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
status_changed = true;
}
hrt_abstime t1 = hrt_absolute_time();
/* publish arming state */
if (arming_state_changed) {
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
/* publish control mode */
if (navigation_state_changed || arming_state_changed) {
/* publish new navigation state */
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
control_mode.counter++;
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.counter++;
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
control_mode.timestamp = t1;