diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-16 15:51:57 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-16 15:51:57 +0100 |
commit | 818ccf7a04fd1aacef4d9e1c6d96493a37302006 (patch) | |
tree | a10a50ab00b8f6d66ca2f727865dbba55f3ba8d8 /src/modules | |
parent | e62bd37e73139c77f0d60cd91fe3443ed23df074 (diff) | |
download | px4-firmware-818ccf7a04fd1aacef4d9e1c6d96493a37302006.tar.gz px4-firmware-818ccf7a04fd1aacef4d9e1c6d96493a37302006.tar.bz2 px4-firmware-818ccf7a04fd1aacef4d9e1c6d96493a37302006.zip |
Commander: Fix for resetting home timestamp on arm
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/commander.cpp | 12 |
1 files changed, 7 insertions, 5 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f3876..bae1ed2e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1860,7 +1860,8 @@ int commander_thread_main(int argc, char *argv[]) } - hrt_abstime t1 = hrt_absolute_time(); + //Get current timestamp + const hrt_abstime now = hrt_absolute_time(); /* print new state */ if (arming_state_changed) { @@ -1871,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - // TODO remove code duplication + // TODO remove code duplication (setting home is also done somewhere else in this file) + home.timestamp = now; home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1932,13 +1934,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); - control_mode.timestamp = t1; + control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - status.timestamp = t1; + status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed.timestamp = t1; + armed.timestamp = now; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } |