mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 00:13:33 +08:00
move baro and magnetometer data out of sensor_combined
This commit is contained in:
@@ -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);
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/att_pos_mocap.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
|
||||
// uORB Publications
|
||||
#include <uORB/Publication.hpp>
|
||||
@@ -260,6 +261,7 @@ private:
|
||||
uORB::Subscription<distance_sensor_s> *_sub_lidar;
|
||||
uORB::Subscription<distance_sensor_s> *_sub_sonar;
|
||||
uORB::Subscription<landing_target_pose_s> _sub_landing_target_pose;
|
||||
uORB::Subscription<vehicle_air_data_s> _sub_airdata;
|
||||
|
||||
// publications
|
||||
uORB::Publication<vehicle_local_position_s> _pub_lpos;
|
||||
|
||||
@@ -41,9 +41,9 @@ int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
|
||||
{
|
||||
//measure
|
||||
y.setZero();
|
||||
y(0) = _sub_sensor.get().baro_alt_meter;
|
||||
y(0) = _sub_airdata.get().baro_alt_meter;
|
||||
_baroStats.update(y);
|
||||
_time_last_baro = _timeStamp;
|
||||
_time_last_baro = _sub_airdata.get().timestamp;
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user