fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 22:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1655 from Zefz/home-timestamp-fix

Browse Source 
Commander: Fix for resetting home timestamp on arm
This commit is contained in:
Lorenz Meier 2015-01-16 16:16:57 +01:00
commit fb730bacd2
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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