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

add ref_timestamp (time when origin was set) to lpe

Browse Source
This commit is contained in:
ChristophTobler
2017-06-07 10:51:37 +02:00
committed by Kabir Mohammed
parent a1a0dd37dc
commit 9b5de23553
4 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -1
View File
@@ -123,6 +123,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_origin(0),
_timeStampLastBaro(hrt_absolute_time()),
_time_last_hist(0),
_time_last_flow(0),
@@ -378,6 +379,9 @@ void BlockLocalPositionEstimator::update()
_init_origin_lat.get(),
_init_origin_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
@@ -610,7 +614,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().yaw = _eul(2);
_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO);
_pub_lpos.get().ref_timestamp = _timeStamp;
_pub_lpos.get().ref_timestamp = _time_origin;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _altOrigin;