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

move baro and magnetometer data out of sensor_combined

Browse Source
This commit is contained in:
Daniel Agar
2018-02-18 11:44:21 -05:00
parent 3e6ba1d541
commit 3b5b12e1d1
31 changed files with 289 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -12
View File
@@ -47,6 +47,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_sub_lidar(nullptr),
_sub_sonar(nullptr),
_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
_sub_airdata(ORB_ID(vehicle_air_data), 0, 0, &getSubscriptions()),
// publications
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
@@ -247,17 +248,10 @@ void BlockLocalPositionEstimator::update()
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
_sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = baro_timestamp;
}
if ((_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = _sub_airdata.get().timestamp;
}
}
@@ -674,7 +668,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().yaw = _eul(2);
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);