diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-13 03:10:23 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-13 03:10:23 -0700 |
commit | db4ff7f080dab08e2558b006ac8c657466c144c6 (patch) | |
tree | e4510f77e1b5820065c1d942b44741c207855718 /src/modules/commander/commander.cpp | |
parent | 90127b2d2a836f75383db8c97d2297333149e6ae (diff) | |
parent | 3fd20481888fd9e63806f988153abe4bf82e7a04 (diff) | |
download | px4-firmware-db4ff7f080dab08e2558b006ac8c657466c144c6.tar.gz px4-firmware-db4ff7f080dab08e2558b006ac8c657466c144c6.tar.bz2 px4-firmware-db4ff7f080dab08e2558b006ac8c657466c144c6.zip |
Merge pull request #446 from PX4/commander_fixes
commander fixes
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 22 |
1 files changed, 5 insertions, 17 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c3546b95..2ef509980 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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(); - 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); + } + + 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; } /* 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; |