fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
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;

1
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -345,6 +345,7 @@ private:
// misc
px4_pollfd_struct_t _polls[3];
uint64_t _timeStamp;
uint64_t _time_origin;
uint64_t _timeStampLastBaro;
uint64_t _time_last_hist;
uint64_t _time_last_flow;

2
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -66,6 +66,8 @@ void BlockLocalPositionEstimator::gpsInit()
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
// reinit origin
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
// always override alt origin on first GPS to fix
// possible baro offset in global altitude at init

2
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -42,6 +42,8 @@ void BlockLocalPositionEstimator::visionInit()
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt));
map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
}
if (!_altOriginInitialized) {