local_position_estimator: move to WQ and delete unused SubscriptionPollable

src/drivers/roboclaw/RoboClaw.hpp

@@ -47,7 +47,6 @@
 #include <stdio.h>
 #include <termios.h>
 #include <lib/parameters/param.h>
-#include <uORB/SubscriptionPollable.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/actuator_armed.h>

src/lib/controllib/block/Block.cpp

@@ -42,8 +42,6 @@
 
 #include <cstring>
 
-#include <uORB/SubscriptionPollable.hpp>
-
 namespace control
 {
 
@@ -99,24 +97,6 @@ void Block::updateParams()
 	updateParamsSubclass();
 }
 
-void Block::updateSubscriptions()
-{
-	uORB::SubscriptionPollableNode *sub = getSubscriptions().getHead();
-	int count = 0;
-
-	while (sub != nullptr) {
-		if (count++ > maxSubscriptionsPerBlock) {
-			char name[blockNameLengthMax];
-			getName(name, blockNameLengthMax);
-			PX4_ERR("exceeded max subscriptions for block: %s", name);
-			break;
-		}
-
-		sub->update();
-		sub = sub->getSibling();
-	}
-}
-
 void SuperBlock::setDt(float dt)
 {
 	Block::setDt(dt);
@@ -154,25 +134,6 @@ void SuperBlock::updateChildParams()
 	}
 }
 
-void SuperBlock::updateChildSubscriptions()
-{
-	Block *child = getChildren().getHead();
-	int count = 0;
-
-	while (child != nullptr) {
-		if (count++ > maxChildrenPerBlock) {
-			char name[blockNameLengthMax];
-			getName(name, blockNameLengthMax);
-			PX4_ERR("exceeded max children for block: %s", name);
-			break;
-		}
-
-		child->updateSubscriptions();
-		child = child->getSibling();
-	}
-}
-
 } // namespace control
 
-template class List<uORB::SubscriptionPollableNode *>;
 template class List<control::BlockParamBase *>;

src/lib/controllib/block/Block.hpp

@@ -40,15 +40,14 @@
 #pragma once
 
 #include <containers/List.hpp>
-#include <uORB/SubscriptionPollable.hpp>
 #include <controllib/block/BlockParam.hpp>
+#include <cstdint>
 
 namespace control
 {
 
 static constexpr uint8_t maxChildrenPerBlock = 100;
 static constexpr uint8_t maxParamsPerBlock = 110;
-static constexpr uint8_t maxSubscriptionsPerBlock = 100;
 static constexpr uint8_t blockNameLengthMax = 40;
 
 // forward declaration
@@ -74,7 +73,6 @@ public:
 	void getName(char *name, size_t n);
 
 	virtual void updateParams();
-	virtual void updateSubscriptions();
 
 	virtual void setDt(float dt) { _dt = dt; }
 	float getDt() { return _dt; }
@@ -84,14 +82,12 @@ protected:
 	virtual void updateParamsSubclass() {}
 
 	SuperBlock *getParent() { return _parent; }
-	List<uORB::SubscriptionPollableNode *> &getSubscriptions() { return _subscriptions; }
 	List<BlockParamBase *> &getParams() { return _params; }
 
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt{0.0f};
 
-	List<uORB::SubscriptionPollableNode *> _subscriptions;
 	List<BlockParamBase *> _params;
 };
 
@@ -119,17 +115,9 @@ public:
 		if (getChildren().getHead() != nullptr) { updateChildParams(); }
 	}
 
-	void updateSubscriptions() override
-	{
-		Block::updateSubscriptions();
-
-		if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); }
-	}
-
 protected:
 	List<Block *> &getChildren() { return _children; }
 	void updateChildParams();
-	void updateChildSubscriptions();
 
 	List<Block *> _children;
 };

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -18,36 +18,12 @@ static const float LAND_RATE = 10.0f;	// rate of land detector correction
 static const char *msg_label = "[lpe] ";	// rate of land detector correction
 
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+
 	// this block has no parent, and has name LPE
 	SuperBlock(nullptr, "LPE"),
-	ModuleParams(nullptr),
+
-	// subscriptions, set rate, add to list
-	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
-	_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
-	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
-	_sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()),
-	// set flow max update rate higher than expected to we don't lose packets
-	_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
-	// main prediction loop, 100 hz
-	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
-	// status updates 2 hz
-	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
-	// gps 10 hz
-	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
-	// vision 50 hz
-	_sub_visual_odom(ORB_ID(vehicle_visual_odometry), 1000 / 50, 0, &getSubscriptions()),
-	// mocap 50 hz
-	_sub_mocap_odom(ORB_ID(vehicle_mocap_odometry), 1000 / 50, 0, &getSubscriptions()),
-	// all distance sensors, 10 hz
-	_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
-	_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
-	_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
-	_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
-	_dist_subs(),
-	_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()),
 	// map projection
 	_map_ref(),
 
@@ -66,6 +42,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 
 	// low pass
 	_xLowPass(this, "X_LP"),
+
 	// use same lp constant for agl
 	_aglLowPass(this, "X_LP"),
 
@@ -74,7 +51,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_tDelay(this, ""),
 
 	// misc
-	_polls(),
 	_timeStamp(hrt_absolute_time()),
 	_time_origin(0),
 	_timeStampLastBaro(hrt_absolute_time()),
@@ -135,22 +111,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_ref_lon(0.0),
 	_ref_alt(0.0)
 {
+	_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz
+
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
 	_dist_subs[1] = &_sub_dist1;
 	_dist_subs[2] = &_sub_dist2;
 	_dist_subs[3] = &_sub_dist3;
 
-	// setup event triggering based on new flow messages to integrate
-	_polls[POLL_FLOW].fd = _sub_flow.getHandle();
-	_polls[POLL_FLOW].events = POLLIN;
-
-	_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
-	_polls[POLL_PARAM].events = POLLIN;
-
-	_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
-	_polls[POLL_SENSORS].events = POLLIN;
-
 	// initialize A, B,  P, x, u
 	_x.setZero();
 	_u.setZero();
@@ -160,33 +128,50 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_map_ref.init_done = false;
 
 	// print fusion settings to console
-	printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
+	PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, "
-	       "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
+		 "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
-	       "baro: %d\n",
+		 "baro: %d\n",
-	       (_param_lpe_fusion.get() & FUSE_GPS) != 0,
+		 (_param_lpe_fusion.get() & FUSE_GPS) != 0,
-	       (_param_lpe_fusion.get() & FUSE_FLOW) != 0,
+		 (_param_lpe_fusion.get() & FUSE_FLOW) != 0,
-	       (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
+		 (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
-	       (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
+		 (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
-	       (_param_lpe_fusion.get() & FUSE_LAND) != 0,
+		 (_param_lpe_fusion.get() & FUSE_LAND) != 0,
-	       (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
+		 (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
-	       (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
+		 (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
-	       (_param_lpe_fusion.get() & FUSE_BARO) != 0);
+		 (_param_lpe_fusion.get() & FUSE_BARO) != 0);
 }
 
+bool
+BlockLocalPositionEstimator::init()
+{
+	if (!_sensors_sub.registerCallback()) {
+		PX4_ERR("sensor combined callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+
 Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
 	float t,
 	const Vector<float, BlockLocalPositionEstimator::n_x> &x,
 	const Vector<float, BlockLocalPositionEstimator::n_u> &u)
 {
-	return _A * x + _B * u;
+	return m_A * x + m_B * u;
 }
 
-void BlockLocalPositionEstimator::update()
+void BlockLocalPositionEstimator::Run()
 {
-	// wait for a sensor update, check for exit condition every 100 ms
+	if (should_exit()) {
-	int ret = px4_poll(_polls, 3, 100);
+		_sensors_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
 
-	if (ret < 0) {
+	sensor_combined_s imu;
+
+	if (!_sensors_sub.update(&imu)) {
 		return;
 	}
 
@@ -198,17 +183,17 @@ void BlockLocalPositionEstimator::update()
 	setDt(dt);
 
 	// auto-detect connected rangefinders while not armed
+	_sub_armed.update();
 	bool armedState = _sub_armed.get().armed;
 
 	if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
 		// detect distance sensors
 		for (size_t i = 0; i < N_DIST_SUBS; i++) {
-			uORB::SubscriptionPollable<distance_sensor_s> *s = _dist_subs[i];
+			auto *s = _dist_subs[i];
 
 			if (s == _sub_lidar || s == _sub_sonar) { continue; }
 
-			if (s->updated()) {
+			if (s->update()) {
-				s->update();
 
 				if (s->get().timestamp == 0) { continue; }
 
@@ -257,27 +242,28 @@ void BlockLocalPositionEstimator::update()
 	_lastArmedState = armedState;
 
 	// see which updates are available
-	bool paramsUpdated = _sub_param_update.updated();
+	bool paramsUpdated = _sub_param_update.update();
 	_baroUpdated = false;
 
-	if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
+	if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update()) {
 		if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
 			_baroUpdated = true;
 			_timeStampLastBaro = _sub_airdata.get().timestamp;
 		}
 	}
 
-	_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
+	_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.update();
-	_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.updated();
+	_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.update();
-	_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
+	_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.update();
-	_mocapUpdated = _sub_mocap_odom.updated();
+	_mocapUpdated = _sub_mocap_odom.update();
-	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
+	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->update();
-	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
+	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->update();
 	_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
-	bool targetPositionUpdated = _sub_landing_target_pose.updated();
+	bool targetPositionUpdated = _sub_landing_target_pose.update();
 
 	// get new data
-	updateSubscriptions();
+	_sub_att.update();
+	_sub_angular_velocity.update();
 
 	// update parameters
 	if (paramsUpdated) {
@@ -289,7 +275,7 @@ void BlockLocalPositionEstimator::update()
 	// is xy valid?
 	bool vxy_stddev_ok = false;
 
-	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
+	if (math::max(m_P(X_vx, X_vx), m_P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
 		vxy_stddev_ok = true;
 	}
 
@@ -314,7 +300,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is z valid?
-	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _param_lpe_z_pub.get();
+	bool z_stddev_ok = sqrtf(m_P(X_z, X_z)) < _param_lpe_z_pub.get();
 
 	if (_estimatorInitialized & EST_Z) {
 		// if valid and baro has timed out, set to not valid
@@ -329,7 +315,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is terrain valid?
-	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
+	bool tz_stddev_ok = sqrtf(m_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
 
 	if (_estimatorInitialized & EST_TZ) {
 		if (!tz_stddev_ok) {
@@ -385,7 +371,7 @@ void BlockLocalPositionEstimator::update()
 
 	for (size_t i = 0; i < n_x; i++) {
 		for (size_t j = 0; j <= i; j++) {
-			if (!PX4_ISFINITE(_P(i, j))) {
+			if (!PX4_ISFINITE(m_P(i, j))) {
 				mavlink_and_console_log_info(&mavlink_log_pub,
 							     "%sreinit P (%zu, %zu) not finite", msg_label, i, j);
 				reinit_P = true;
@@ -393,7 +379,7 @@ void BlockLocalPositionEstimator::update()
 
 			if (i == j) {
 				// make sure diagonal elements are positive
-				if (_P(i, i) <= 0) {
+				if (m_P(i, i) <= 0) {
 					mavlink_and_console_log_info(&mavlink_log_pub,
 								     "%sreinit P (%zu, %zu) negative", msg_label, i, j);
 					reinit_P = true;
@@ -402,7 +388,7 @@ void BlockLocalPositionEstimator::update()
 			} else {
 				// copy elememnt from upper triangle to force
 				// symmetry
-				_P(j, i) = _P(i, j);
+				m_P(j, i) = m_P(i, j);
 			}
 
 			if (reinit_P) { break; }
@@ -416,7 +402,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// do prediction
-	predict();
+	predict(imu);
 
 	// sensor corrections/ initializations
 	if (_gpsUpdated) {
@@ -545,6 +531,8 @@ bool BlockLocalPositionEstimator::landed()
 		return false;
 	}
 
+	_sub_land.update();
+
 	bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);
 
 	return _sub_land.get().landed || disarmed_not_falling;
@@ -555,10 +543,10 @@ void BlockLocalPositionEstimator::publishLocalPos()
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
 
 	// lie about eph/epv to allow visual odometry only navigation when velocity est. good
-	float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
+	float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
-	float evv = sqrtf(_P(X_vz, X_vz));
+	float evv = sqrtf(m_P(X_vz, X_vz));
-	float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
+	float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
-	float epv = sqrtf(_P(X_z, X_z));
+	float epv = sqrtf(m_P(X_z, X_z));
 
 	float eph_thresh = 3.0f;
 	float epv_thresh = 3.0f;
@@ -679,9 +667,9 @@ void BlockLocalPositionEstimator::publishOdom()
 		}
 
 		// set the position variances
-		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = _P(X_vx, X_vx);
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = m_P(X_vx, X_vx);
-		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = _P(X_vy, X_vy);
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = m_P(X_vy, X_vy);
-		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = _P(X_vz, X_vz);
+		_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = m_P(X_vz, X_vz);
 
 		// unknown orientation covariances
 		// TODO: add orientation covariance to vehicle_attitude
@@ -695,9 +683,9 @@ void BlockLocalPositionEstimator::publishOdom()
 		}
 
 		// set the linear velocity variances
-		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = _P(X_vx, X_vx);
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = m_P(X_vx, X_vx);
-		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = _P(X_vy, X_vy);
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = m_P(X_vy, X_vy);
-		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = _P(X_vz, X_vz);
+		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz);
 
 		// unknown angular velocity covariances
 		_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
@@ -723,21 +711,21 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_status.get().covariances[2] = NAN;
 	_pub_est_status.get().covariances[3] = NAN;
 	// linear velocity
-	_pub_est_status.get().covariances[4] = _P(X_vx, X_vx);
+	_pub_est_status.get().covariances[4] = m_P(X_vx, X_vx);
-	_pub_est_status.get().covariances[5] = _P(X_vy, X_vy);
+	_pub_est_status.get().covariances[5] = m_P(X_vy, X_vy);
-	_pub_est_status.get().covariances[6] = _P(X_vz, X_vz);
+	_pub_est_status.get().covariances[6] = m_P(X_vz, X_vz);
 	// position
-	_pub_est_status.get().covariances[7] = _P(X_x, X_x);
+	_pub_est_status.get().covariances[7] = m_P(X_x, X_x);
-	_pub_est_status.get().covariances[8] = _P(X_y, X_y);
+	_pub_est_status.get().covariances[8] = m_P(X_y, X_y);
-	_pub_est_status.get().covariances[9] = _P(X_z, X_z);
+	_pub_est_status.get().covariances[9] = m_P(X_z, X_z);
 	// gyro bias - not determined
 	_pub_est_status.get().covariances[10] = NAN;
 	_pub_est_status.get().covariances[11] = NAN;
 	_pub_est_status.get().covariances[12] = NAN;
 	// accel bias
-	_pub_est_status.get().covariances[13] = _P(X_bx, X_bx);
+	_pub_est_status.get().covariances[13] = m_P(X_bx, X_bx);
-	_pub_est_status.get().covariances[14] = _P(X_by, X_by);
+	_pub_est_status.get().covariances[14] = m_P(X_by, X_by);
-	_pub_est_status.get().covariances[15] = _P(X_bz, X_bz);
+	_pub_est_status.get().covariances[15] = m_P(X_bz, X_bz);
 
 	// mag - not determined
 	for (size_t i = 16; i <= 21; i++) {
@@ -745,7 +733,7 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	}
 
 	// replacing the hor wind cov with terrain altitude covariance
-	_pub_est_status.get().covariances[22] = _P(X_tz, X_tz);
+	_pub_est_status.get().covariances[22] = m_P(X_tz, X_tz);
 	_pub_est_status.get().covariances[23] = NAN;
 
 	_pub_est_status.get().n_states = n_x;
@@ -767,9 +755,9 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	float alt = -xLP(X_z) + _altOrigin;
 
 	// lie about eph/epv to allow visual odometry only navigation when velocity est. good
-	float evh = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
+	float evh = sqrtf(m_P(X_vx, X_vx) + m_P(X_vy, X_vy));
-	float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
+	float eph = sqrtf(m_P(X_x, X_x) + m_P(X_y, X_y));
-	float epv = sqrtf(_P(X_z, X_z));
+	float epv = sqrtf(m_P(X_z, X_z));
 
 	float eph_thresh = 3.0f;
 	float epv_thresh = 3.0f;
@@ -806,20 +794,20 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 
 void BlockLocalPositionEstimator::initP()
 {
-	_P.setZero();
+	m_P.setZero();
 	// initialize to twice valid condition
-	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
+	m_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
-	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
+	m_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
-	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
+	m_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
-	_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
+	m_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
-	_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
+	m_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
 	// use vxy thresh for vz init as well
-	_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
+	m_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
 	// initialize bias uncertainty to small values to keep them stable
-	_P(X_bx, X_bx) = 1e-6;
+	m_P(X_bx, X_bx) = 1e-6;
-	_P(X_by, X_by) = 1e-6;
+	m_P(X_by, X_by) = 1e-6;
-	_P(X_bz, X_bz) = 1e-6;
+	m_P(X_bz, X_bz) = 1e-6;
-	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
+	m_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;
 }
 
 void BlockLocalPositionEstimator::initSS()
@@ -827,17 +815,17 @@ void BlockLocalPositionEstimator::initSS()
 	initP();
 
 	// dynamics matrix
-	_A.setZero();
+	m_A.setZero();
 	// derivative of position is velocity
-	_A(X_x, X_vx) = 1;
+	m_A(X_x, X_vx) = 1;
-	_A(X_y, X_vy) = 1;
+	m_A(X_y, X_vy) = 1;
-	_A(X_z, X_vz) = 1;
+	m_A(X_z, X_vz) = 1;
 
 	// input matrix
-	_B.setZero();
+	m_B.setZero();
-	_B(X_vx, U_ax) = 1;
+	m_B(X_vx, U_ax) = 1;
-	_B(X_vy, U_ay) = 1;
+	m_B(X_vy, U_ay) = 1;
-	_B(X_vz, U_az) = 1;
+	m_B(X_vz, U_az) = 1;
 
 	// update components that depend on current state
 	updateSSStates();
@@ -848,58 +836,58 @@ void BlockLocalPositionEstimator::updateSSStates()
 {
 	// derivative of velocity is accelerometer acceleration
 	// (in input matrix) - bias (in body frame)
-	_A(X_vx, X_bx) = -_R_att(0, 0);
+	m_A(X_vx, X_bx) = -_R_att(0, 0);
-	_A(X_vx, X_by) = -_R_att(0, 1);
+	m_A(X_vx, X_by) = -_R_att(0, 1);
-	_A(X_vx, X_bz) = -_R_att(0, 2);
+	m_A(X_vx, X_bz) = -_R_att(0, 2);
 
-	_A(X_vy, X_bx) = -_R_att(1, 0);
+	m_A(X_vy, X_bx) = -_R_att(1, 0);
-	_A(X_vy, X_by) = -_R_att(1, 1);
+	m_A(X_vy, X_by) = -_R_att(1, 1);
-	_A(X_vy, X_bz) = -_R_att(1, 2);
+	m_A(X_vy, X_bz) = -_R_att(1, 2);
 
-	_A(X_vz, X_bx) = -_R_att(2, 0);
+	m_A(X_vz, X_bx) = -_R_att(2, 0);
-	_A(X_vz, X_by) = -_R_att(2, 1);
+	m_A(X_vz, X_by) = -_R_att(2, 1);
-	_A(X_vz, X_bz) = -_R_att(2, 2);
+	m_A(X_vz, X_bz) = -_R_att(2, 2);
 }
 
 void BlockLocalPositionEstimator::updateSSParams()
 {
 	// input noise covariance matrix
-	_R.setZero();
+	m_R.setZero();
-	_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
+	m_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
-	_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
+	m_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
-	_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
+	m_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
 
 	// process noise power matrix
-	_Q.setZero();
+	m_Q.setZero();
 	float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
 	float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
-	_Q(X_x, X_x) = pn_p_sq;
+	m_Q(X_x, X_x) = pn_p_sq;
-	_Q(X_y, X_y) = pn_p_sq;
+	m_Q(X_y, X_y) = pn_p_sq;
-	_Q(X_z, X_z) = pn_p_sq;
+	m_Q(X_z, X_z) = pn_p_sq;
-	_Q(X_vx, X_vx) = pn_v_sq;
+	m_Q(X_vx, X_vx) = pn_v_sq;
-	_Q(X_vy, X_vy) = pn_v_sq;
+	m_Q(X_vy, X_vy) = pn_v_sq;
-	_Q(X_vz, X_vz) = pn_v_sq;
+	m_Q(X_vz, X_vz) = pn_v_sq;
 
 	// technically, the noise is in the body frame,
 	// but the components are all the same, so
 	// ignoring for now
 	float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
-	_Q(X_bx, X_bx) = pn_b_sq;
+	m_Q(X_bx, X_bx) = pn_b_sq;
-	_Q(X_by, X_by) = pn_b_sq;
+	m_Q(X_by, X_by) = pn_b_sq;
-	_Q(X_bz, X_bz) = pn_b_sq;
+	m_Q(X_bz, X_bz) = pn_b_sq;
 
 	// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
 	float pn_t_noise_density =
 		_param_lpe_pn_t.get() +
 		(_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
-	_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
+	m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
 }
 
-void BlockLocalPositionEstimator::predict()
+void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu)
 {
 	// get acceleration
 	_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
-	Vector3f a(_sub_sensor.get().accelerometer_m_s2);
+	Vector3f a(imu.accelerometer_m_s2);
 	// note, bias is removed in dynamics function
 	_u = _R_att * a;
 	_u(U_az) += CONSTANTS_ONE_G;	// add g
@@ -959,12 +947,12 @@ void BlockLocalPositionEstimator::predict()
 
 	// propagate
 	_x += dx;
-	Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
+	Matrix<float, n_x, n_x> dP = (m_A * m_P + m_P * m_A.transpose() +
-				      _B * _R * _B.transpose() + _Q) * getDt();
+				      m_B * m_R * m_B.transpose() + m_Q) * getDt();
 
 	// covariance propagation logic
 	for (size_t i = 0; i < n_x; i++) {
-		if (_P(i, i) > P_MAX) {
+		if (m_P(i, i) > P_MAX) {
 			// if diagonal element greater than max, stop propagating
 			dP(i, i) = 0;
 
@@ -975,7 +963,7 @@ void BlockLocalPositionEstimator::predict()
 		}
 	}
 
-	_P += dP;
+	m_P += dP;
 	_xLowPass.update(_x);
 	_aglLowPass.update(agl());
 }
@@ -1002,3 +990,59 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
 
 	return OK;
 }
+
+int
+BlockLocalPositionEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int
+BlockLocalPositionEstimator::task_spawn(int argc, char *argv[])
+{
+	BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int
+BlockLocalPositionEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Attitude and position estimator using an Extended Kalman Filter.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[])
+{
+	return BlockLocalPositionEstimator::main(argc, argv);
+}

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -1,15 +1,17 @@
 #pragma once
 
-#include <px4_platform_common/posix.h>
 #include <drivers/drv_hrt.h>
+#include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <controllib/blocks.hpp>
+#include <px4_platform_common/posix.h>
-#include <mathlib/mathlib.h>
+#include <lib/controllib/blocks.hpp>
 #include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
 #include <matrix/Matrix.hpp>
 
 // uORB Subscriptions
-#include <uORB/SubscriptionPollable.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -36,6 +38,7 @@
 
 using namespace matrix;
 using namespace control;
+using namespace time_literals;
 
 static const float DELAY_MAX = 0.5f;	// seconds
 static const float HIST_STEP = 0.05f;	// 20 hz
@@ -56,7 +59,8 @@ static const float BETA_TABLE[7] = {0,
 				    19.6465647819,
 				   };
 
-class BlockLocalPositionEstimator : public control::SuperBlock, public ModuleParams
+class BlockLocalPositionEstimator : public ModuleBase<BlockLocalPositionEstimator>, public ModuleParams,
+	public px4::WorkItem, public control::SuperBlock
 {
 // dynamics:
 //
@@ -106,8 +110,21 @@ class BlockLocalPositionEstimator : public control::SuperBlock, public ModulePar
 //
 public:
 
-	BlockLocalPositionEstimator(const BlockLocalPositionEstimator &) = delete;
+	BlockLocalPositionEstimator();
-	BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &) = delete;
+	~BlockLocalPositionEstimator() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
 
 	// constants
 	enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
@@ -121,7 +138,6 @@ public:
 	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
 	enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
 	enum {Y_target_x = 0, Y_target_y, n_y_target};
-	enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
 	enum {
 		FUSE_GPS = 1 << 0,
 		FUSE_FLOW = 1 << 1,
@@ -151,12 +167,7 @@ public:
 		EST_TZ = 1 << 2,
 	};
 
-	// public methods
+	void Run() override;
-	BlockLocalPositionEstimator();
-	void update();
-	virtual ~BlockLocalPositionEstimator() = default;
-
-private:
 
 	// methods
 	// ----------------------------
@@ -171,7 +182,7 @@ private:
 	void updateSSParams();
 
 	// predict the next state
-	void predict();
+	void predict(const sensor_combined_s &imu);
 
 	// lidar
 	int  lidarMeasure(Vector<float, n_y_lidar> &y);
@@ -248,25 +259,26 @@ private:
 	// ----------------------------
 
 	// subscriptions
-	uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
+	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
-	uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
+
-	uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
+	uORB::SubscriptionData<actuator_armed_s> _sub_armed{ORB_ID(actuator_armed)};
-	uORB::SubscriptionPollable<vehicle_angular_velocity_s> _sub_angular_velocity;
+	uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
-	uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
+	uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
-	uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
+	uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
-	uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;
+	uORB::SubscriptionData<optical_flow_s> _sub_flow{ORB_ID(optical_flow)};
-	uORB::SubscriptionPollable<vehicle_gps_position_s> _sub_gps;
+	uORB::SubscriptionData<parameter_update_s> _sub_param_update{ORB_ID(parameter_update)};
-	uORB::SubscriptionPollable<vehicle_odometry_s> _sub_visual_odom;
+	uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
-	uORB::SubscriptionPollable<vehicle_odometry_s> _sub_mocap_odom;
+	uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
-	uORB::SubscriptionPollable<distance_sensor_s> _sub_dist0;
+	uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};
-	uORB::SubscriptionPollable<distance_sensor_s> _sub_dist1;
+	uORB::SubscriptionData<distance_sensor_s> _sub_dist0{ORB_ID(distance_sensor), 0};
-	uORB::SubscriptionPollable<distance_sensor_s> _sub_dist2;
+	uORB::SubscriptionData<distance_sensor_s> _sub_dist1{ORB_ID(distance_sensor), 1};
-	uORB::SubscriptionPollable<distance_sensor_s> _sub_dist3;
+	uORB::SubscriptionData<distance_sensor_s> _sub_dist2{ORB_ID(distance_sensor), 2};
-	uORB::SubscriptionPollable<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
+	uORB::SubscriptionData<distance_sensor_s> _sub_dist3{ORB_ID(distance_sensor), 3};
-	uORB::SubscriptionPollable<distance_sensor_s> *_sub_lidar;
+	uORB::SubscriptionData<distance_sensor_s> *_dist_subs[N_DIST_SUBS] {};
-	uORB::SubscriptionPollable<distance_sensor_s> *_sub_sonar;
+	uORB::SubscriptionData<distance_sensor_s> *_sub_lidar{nullptr};
-	uORB::SubscriptionPollable<landing_target_pose_s> _sub_landing_target_pose;
+	uORB::SubscriptionData<distance_sensor_s> *_sub_sonar{nullptr};
-	uORB::SubscriptionPollable<vehicle_air_data_s> _sub_airdata;
+	uORB::SubscriptionData<landing_target_pose_s> _sub_landing_target_pose{ORB_ID(landing_target_pose)};
+	uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata{ORB_ID(vehicle_air_data)};
 
 	// publications
 	uORB::PublicationData<vehicle_local_position_s> _pub_lpos{ORB_ID(vehicle_local_position)};
@@ -278,6 +290,107 @@ private:
 	// map projection
 	struct map_projection_reference_s _map_ref;
 
+	// target mode paramters from landing_target_estimator module
+	enum TargetMode {
+		Target_Moving = 0,
+		Target_Stationary = 1
+	};
+
+	// flow gyro filter
+	BlockHighPass _flow_gyro_x_high_pass;
+	BlockHighPass _flow_gyro_y_high_pass;
+
+	// stats
+	BlockStats<float, n_y_baro> _baroStats;
+	BlockStats<float, n_y_sonar> _sonarStats;
+	BlockStats<float, n_y_lidar> _lidarStats;
+	BlockStats<float, 1> _flowQStats;
+	BlockStats<float, n_y_vision> _visionStats;
+	BlockStats<float, n_y_mocap> _mocapStats;
+	BlockStats<double, n_y_gps> _gpsStats;
+	uint16_t _landCount;
+
+	// low pass
+	BlockLowPassVector<float, n_x> _xLowPass;
+	BlockLowPass _aglLowPass;
+
+	// delay blocks
+	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
+	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
+
+	// misc
+	uint64_t _timeStamp;
+	uint64_t _time_origin;
+	uint64_t _timeStampLastBaro;
+	uint64_t _time_last_hist;
+	uint64_t _time_last_flow;
+	uint64_t _time_last_baro;
+	uint64_t _time_last_gps;
+	uint64_t _time_last_lidar;
+	uint64_t _time_last_sonar;
+	uint64_t _time_init_sonar;
+	uint64_t _time_last_vision_p;
+	uint64_t _time_last_mocap;
+	uint64_t _time_last_land;
+	uint64_t _time_last_target;
+
+	// reference altitudes
+	float _altOrigin;
+	bool _altOriginInitialized;
+	bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
+	float _baroAltOrigin;
+	float _gpsAltOrigin;
+
+	// status
+	bool _receivedGps;
+	bool _lastArmedState;
+
+	// masks
+	uint16_t _sensorTimeout;
+	uint16_t _sensorFault;
+	uint8_t _estimatorInitialized;
+
+	// sensor update flags
+	bool _flowUpdated;
+	bool _gpsUpdated;
+	bool _visionUpdated;
+	bool _mocapUpdated;
+	bool _lidarUpdated;
+	bool _sonarUpdated;
+	bool _landUpdated;
+	bool _baroUpdated;
+
+	// sensor validation flags
+	bool _vision_xy_valid;
+	bool _vision_z_valid;
+	bool _mocap_xy_valid;
+	bool _mocap_z_valid;
+
+	// sensor std deviations
+	float _vision_eph;
+	float _vision_epv;
+	float _mocap_eph;
+	float _mocap_epv;
+
+	// local to global coversion related variables
+	bool _is_global_cov_init;
+	uint64_t _global_ref_timestamp;
+	double _ref_lat;
+	double _ref_lon;
+	float _ref_alt;
+
+	// state space
+	Vector<float, n_x>  _x;	// state vector
+	Vector<float, n_u>  _u;	// input vector
+	Matrix<float, n_x, n_x> m_P;	// state covariance matrix
+
+	matrix::Dcm<float> _R_att;
+
+	Matrix<float, n_x, n_x>  m_A;	// dynamics matrix
+	Matrix<float, n_x, n_u>  m_B;	// input matrix
+	Matrix<float, n_u, n_u>  m_R;	// input covariance
+	Matrix<float, n_x, n_x>  m_Q;	// process noise covariance
+
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
@@ -346,105 +459,4 @@ private:
 		(ParamFloat<px4::params::LPE_LON>) _param_lpe_lon
 	)
 
-	// target mode paramters from landing_target_estimator module
-	enum TargetMode {
-		Target_Moving = 0,
-		Target_Stationary = 1
-	};
-
-	// flow gyro filter
-	BlockHighPass _flow_gyro_x_high_pass;
-	BlockHighPass _flow_gyro_y_high_pass;
-
-	// stats
-	BlockStats<float, n_y_baro> _baroStats;
-	BlockStats<float, n_y_sonar> _sonarStats;
-	BlockStats<float, n_y_lidar> _lidarStats;
-	BlockStats<float, 1> _flowQStats;
-	BlockStats<float, n_y_vision> _visionStats;
-	BlockStats<float, n_y_mocap> _mocapStats;
-	BlockStats<double, n_y_gps> _gpsStats;
-	uint16_t _landCount;
-
-	// low pass
-	BlockLowPassVector<float, n_x> _xLowPass;
-	BlockLowPass _aglLowPass;
-
-	// delay blocks
-	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
-	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
-
-	// 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;
-	uint64_t _time_last_baro;
-	uint64_t _time_last_gps;
-	uint64_t _time_last_lidar;
-	uint64_t _time_last_sonar;
-	uint64_t _time_init_sonar;
-	uint64_t _time_last_vision_p;
-	uint64_t _time_last_mocap;
-	uint64_t _time_last_land;
-	uint64_t _time_last_target;
-
-	// reference altitudes
-	float _altOrigin;
-	bool _altOriginInitialized;
-	bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
-	float _baroAltOrigin;
-	float _gpsAltOrigin;
-
-	// status
-	bool _receivedGps;
-	bool _lastArmedState;
-
-	// masks
-	uint16_t _sensorTimeout;
-	uint16_t _sensorFault;
-	uint8_t _estimatorInitialized;
-
-	// sensor update flags
-	bool _flowUpdated;
-	bool _gpsUpdated;
-	bool _visionUpdated;
-	bool _mocapUpdated;
-	bool _lidarUpdated;
-	bool _sonarUpdated;
-	bool _landUpdated;
-	bool _baroUpdated;
-
-	// sensor validation flags
-	bool _vision_xy_valid;
-	bool _vision_z_valid;
-	bool _mocap_xy_valid;
-	bool _mocap_z_valid;
-
-	// sensor std deviations
-	float _vision_eph;
-	float _vision_epv;
-	float _mocap_eph;
-	float _mocap_epv;
-
-	// local to global coversion related variables
-	bool _is_global_cov_init;
-	uint64_t _global_ref_timestamp;
-	double _ref_lat;
-	double _ref_lon;
-	float _ref_alt;
-
-	// state space
-	Vector<float, n_x>  _x;	// state vector
-	Vector<float, n_u>  _u;	// input vector
-	Matrix<float, n_x, n_x>  _P;	// state covariance matrix
-
-	matrix::Dcm<float> _R_att;
-
-	Matrix<float, n_x, n_x>  _A;	// dynamics matrix
-	Matrix<float, n_x, n_u>  _B;	// input matrix
-	Matrix<float, n_u, n_u>  _R;	// input covariance
-	Matrix<float, n_x, n_x>  _Q;	// process noise covariance
 };

src/modules/local_position_estimator/CMakeLists.txt

@@ -34,11 +34,9 @@ px4_add_module(
 	MODULE modules__local_position_estimator
 	MAIN local_position_estimator
 	COMPILE_FLAGS
-		-Wno-double-promotion # TODO: fix in Matrix library Vector::pow()
 	STACK_MAIN 5700
-	STACK_MAX 13000
+	STACK_MAX 3700
 	SRCS
-		local_position_estimator_main.cpp
 		BlockLocalPositionEstimator.cpp
 		sensors/flow.cpp
 		sensors/lidar.cpp
@@ -53,4 +51,5 @@ px4_add_module(
 		controllib
 		git_ecl
 		ecl_geo
+		px4_work_queue
 	)

src/modules/local_position_estimator/local_position_estimator_main.cpp

@@ -1,140 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file local_position_estimator.cpp
- * @author James Goppert <james.goppert@gmail.com>
- * @author Mohammed Kabir
- * @author Nuno Marques <n.marques21@hotmail.com>
- *
- * Local position estimator
- */
-
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/tasks.h>
-
-#include "BlockLocalPositionEstimator.hpp"
-
-extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
-
-class LocalPositionEstimatorModule : public ModuleBase<LocalPositionEstimatorModule>
-{
-public:
-	~LocalPositionEstimatorModule() override = default;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static LocalPositionEstimatorModule *instantiate(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	/** @see ModuleBase::run() */
-	void run() override;
-
-private:
-	BlockLocalPositionEstimator _estimator;
-};
-
-int LocalPositionEstimatorModule::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Attitude and position estimator using an Extended Kalman Filter.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("local_position_estimator", "estimator");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-int LocalPositionEstimatorModule::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-
-int LocalPositionEstimatorModule::task_spawn(int argc, char *argv[])
-{
-		_task_id = px4_task_spawn_cmd("lp_estimator",
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_ESTIMATOR,
-						 7900,
-						 (px4_main_t)&run_trampoline,
-						 (char *const *)argv);
-
-	if (_task_id < 0) {
-		_task_id = -1;
-		return -errno;
-	}
-
-	return 0;
-}
-
-LocalPositionEstimatorModule *LocalPositionEstimatorModule::instantiate(int argc, char *argv[])
-{
-	LocalPositionEstimatorModule *instance = new LocalPositionEstimatorModule();
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-	}
-
-	return instance;
-}
-
-void LocalPositionEstimatorModule::run()
-{
-	while (!should_exit()) {
-		_estimator.update();
-	}
-}
-
-
-int local_position_estimator_main(int argc, char *argv[])
-{
-	return LocalPositionEstimatorModule::main(argc, argv);
-}

src/modules/local_position_estimator/sensors/baro.cpp

@@ -68,7 +68,7 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	// residual
 	Matrix<float, n_y_baro, n_y_baro> S_I =
-		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
+		inv<float, n_y_baro>((C * m_P * C.transpose()) + R);
 	Vector<float, n_y_baro> r = y - (C * _x);
 
 	// fault detection
@@ -87,10 +87,10 @@ void BlockLocalPositionEstimator::baroCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_baro> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::baroCheckTimeout()

src/modules/local_position_estimator/sensors/flow.cpp

@@ -169,7 +169,7 @@ void BlockLocalPositionEstimator::flowCorrect()
 	Vector<float, 2> r = y - C * _x;
 
 	// residual covariance
-	Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_flow, n_y_flow> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().flow_innov[0] = r(0);
@@ -196,10 +196,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	if (!(_sensorFault & SENSOR_FLOW)) {
 		Matrix<float, n_x, n_y_flow> K =
-			_P * C.transpose() * S_I;
+			m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }
 

src/modules/local_position_estimator/sensors/gps.cpp

@@ -186,7 +186,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	Vector<float, n_y_gps> r = y - C * x0;
 
 	// residual covariance
-	Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	for (size_t i = 0; i < 6; i++) {
@@ -217,10 +217,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	}
 
 	// kalman filter correction always for GPS
-	Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_gps> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::gpsCheckTimeout()

src/modules/local_position_estimator/sensors/land.cpp

@@ -59,7 +59,7 @@ void BlockLocalPositionEstimator::landCorrect()
 	R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get();
 
 	// residual
-	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
+	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
 	Vector<float, n_y_land> r = y - C * _x;
 	_pub_innov.get().hagl_innov = r(Y_land_agl);
 	_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
@@ -85,10 +85,10 @@ void BlockLocalPositionEstimator::landCorrect()
 	}
 
 	// kalman filter correction always for land detector
-	Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_land> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::landCheckTimeout()

src/modules/local_position_estimator/sensors/landing_target.cpp

@@ -82,7 +82,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_target, n_y_target> S_I =
-		inv<float, n_y_target>(C * _P * C.transpose() + R);
+		inv<float, n_y_target>(C * m_P * C.transpose() + R);
 
 	// fault detection
 	float beta = (r.transpose()  * (S_I * r))(0, 0);
@@ -103,10 +103,10 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
 
 	// kalman filter correction
 	Matrix<float, n_x, n_y_target> K =
-		_P * C.transpose() * S_I;
+		m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 
 }
 

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	// residual
 	Vector<float, n_y_lidar> r = y - C * _x;
 	// residual covariance
-	Matrix<float, n_y_lidar, n_y_lidar> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
@@ -116,10 +116,10 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_lidar> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::lidarCheckTimeout()

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -106,7 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	Vector<float, n_y_mocap> y;
 
 	if (mocapMeasure(y) != OK) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv);
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
+					     (double)_mocap_epv);
 		return;
 	}
 
@@ -141,7 +142,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// residual
 	Vector<float, n_y_mocap> r = y - C * _x;
 	// residual covariance
-	Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	for (size_t i = 0; i < 3; i++) {
@@ -172,10 +173,10 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	}
 
 	// kalman filter correction always
-	Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
+	Matrix<float, n_x, n_y_mocap> K = m_P * C.transpose() * S_I;
 	Vector<float, n_x> dx = K * r;
 	_x += dx;
-	_P -= K * C * _P;
+	m_P -= K * C * m_P;
 }
 
 void BlockLocalPositionEstimator::mocapCheckTimeout()

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -109,7 +109,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	// residual
 	Vector<float, n_y_sonar> r = y - C * _x;
 	// residual covariance
-	Matrix<float, n_y_sonar, n_y_sonar> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	_pub_innov.get().hagl_innov = r(0);
@@ -138,10 +138,10 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	// kalman filter correction if no fault
 	if (!(_sensorFault & SENSOR_SONAR)) {
 		Matrix<float, n_x, n_y_sonar> K =
-			_P * C.transpose() * S_I;
+			m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }
 

src/modules/local_position_estimator/sensors/vision.cpp

@@ -111,7 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Vector<float, n_y_vision> y;
 
 	if (visionMeasure(y) != OK) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", _vision_eph, _vision_epv);
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
+					     (double)_vision_epv);
 		return;
 	}
 
@@ -158,7 +159,7 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// residual
 	Matrix<float, n_y_vision, 1> r = y - C * x0;
 	// residual covariance
-	Matrix<float, n_y_vision, n_y_vision> S = C * _P * C.transpose() + R;
+	Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
 	for (size_t i = 0; i < 3; i++) {
@@ -190,10 +191,10 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	// kalman filter correction if no fault
 	if (!(_sensorFault & SENSOR_VISION)) {
-		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
+		Matrix<float, n_x, n_y_vision> K = m_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
 		_x += dx;
-		_P -= K * C * _P;
+		m_P -= K * C * m_P;
 	}
 }
 

src/modules/uORB/CMakeLists.txt

@@ -48,8 +48,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 			Subscription.hpp
 			SubscriptionCallback.hpp
 			SubscriptionInterval.hpp
-			SubscriptionPollable.cpp
-			SubscriptionPollable.hpp
 			uORB.cpp
 			uORB.h
 			uORBCommon.hpp

src/modules/uORB/SubscriptionPollable.cpp

@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file SubscriptionPollable.cpp
- *
- */
-
-#include "SubscriptionPollable.hpp"
-#include <px4_platform_common/defines.h>
-
-namespace uORB
-{
-
-SubscriptionPollableBase::SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval,
-		unsigned instance) :
-	_meta(meta),
-	_instance(instance)
-{
-	if (instance > 0) {
-		_handle = orb_subscribe_multi(_meta, instance);
-
-	} else {
-		_handle = orb_subscribe(_meta);
-	}
-
-	if (_handle < 0) {
-		PX4_ERR("%s sub failed", _meta->o_name);
-	}
-
-	if (interval > 0) {
-		orb_set_interval(_handle, interval);
-	}
-}
-
-bool SubscriptionPollableBase::updated()
-{
-	bool isUpdated = false;
-
-	if (orb_check(_handle, &isUpdated) != PX4_OK) {
-		PX4_ERR("%s check failed", _meta->o_name);
-	}
-
-	return isUpdated;
-}
-
-bool SubscriptionPollableBase::update(void *data)
-{
-	bool orb_updated = false;
-
-	if (updated()) {
-		if (orb_copy(_meta, _handle, data) != PX4_OK) {
-			PX4_ERR("%s copy failed", _meta->o_name);
-
-		} else {
-			orb_updated = true;
-		}
-	}
-
-	return orb_updated;
-}
-
-SubscriptionPollableBase::~SubscriptionPollableBase()
-{
-	if (orb_unsubscribe(_handle) != PX4_OK) {
-		PX4_ERR("%s unsubscribe failed", _meta->o_name);
-	}
-}
-
-SubscriptionPollableNode::SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval,
-		unsigned instance,
-		List<SubscriptionPollableNode *> *list)
-	: SubscriptionPollableBase(meta, interval, instance)
-{
-	if (list != nullptr) {
-		list->add(this);
-	}
-}
-
-} // namespace uORB

src/modules/uORB/SubscriptionPollable.hpp

@@ -1,197 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file SubscriptionPollable.hpp
- *
- */
-
-#pragma once
-
-#include <uORB/uORB.h>
-#include <containers/List.hpp>
-#include <systemlib/err.h>
-#include <px4_platform_common/defines.h>
-
-namespace uORB
-{
-
-/**
- * Base subscription wrapper class, used in list traversal
- * of various subscriptions.
- */
-class __EXPORT SubscriptionPollableBase
-{
-public:
-	/**
-	 * Constructor
-	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
-	 * @param instance The instance for multi sub.
-	 */
-	SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
-	virtual ~SubscriptionPollableBase();
-
-	// no copy, assignment, move, move assignment
-	SubscriptionPollableBase(const SubscriptionPollableBase &) = delete;
-	SubscriptionPollableBase &operator=(const SubscriptionPollableBase &) = delete;
-	SubscriptionPollableBase(SubscriptionPollableBase &&) = delete;
-	SubscriptionPollableBase &operator=(SubscriptionPollableBase &&) = delete;
-
-	/**
-	 * Check if there is a new update.
-	 * */
-	bool updated();
-
-	/**
-	 * Update the struct
-	 * @param data The uORB message struct we are updating.
-	 */
-	bool update(void *data);
-
-	int getHandle() const { return _handle; }
-
-	const orb_metadata *getMeta() const { return _meta; }
-
-	unsigned getInstance() const { return _instance; }
-
-protected:
-	const struct orb_metadata *_meta;
-
-	unsigned _instance;
-
-	int _handle{-1};
-};
-
-/**
- * alias class name so it is clear that the base class
- */
-typedef SubscriptionPollableBase SubscriptionPollableTiny;
-
-/**
- * The subscription base class as a list node.
- */
-class __EXPORT SubscriptionPollableNode : public SubscriptionPollableBase, public ListNode<SubscriptionPollableNode *>
-{
-public:
-	/**
-	 * Constructor
-	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
-	 * @param instance The instance for multi sub.
-	 * @param list 	A pointer to a list of subscriptions
-	 * 	that this should be appended to.
-	 */
-	SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
-				 List<SubscriptionPollableNode *> *list = nullptr);
-
-	virtual ~SubscriptionPollableNode() override = default;
-
-	/**
-	 * This function is the callback for list traversal
-	 * updates, a child class must implement it.
-	 */
-	virtual bool update() = 0;
-
-	/**
-	 * Like update(), but does not check first if there is data available
-	 */
-	virtual bool forcedUpdate() = 0;
-
-};
-
-/**
- * SubscriptionPollable wrapper class
- */
-template<class T>
-class __EXPORT SubscriptionPollable : public SubscriptionPollableNode
-{
-public:
-	/**
-	 * Constructor
-	 *
-	 * @param meta The uORB metadata (usually from
-	 * 	the ORB_ID() macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
-	 * @param list A list interface for adding to
-	 * 	list during construction
-	 */
-	SubscriptionPollable(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
-			     List<SubscriptionPollableNode *> *list = nullptr):
-		SubscriptionPollableNode(meta, interval, instance, list),
-		_data() // initialize data structure to zero
-	{
-		forcedUpdate();
-	}
-
-	~SubscriptionPollable() override = default;
-
-	// no copy, assignment, move, move assignment
-	SubscriptionPollable(const SubscriptionPollable &) = delete;
-	SubscriptionPollable &operator=(const SubscriptionPollable &) = delete;
-	SubscriptionPollable(SubscriptionPollable &&) = delete;
-	SubscriptionPollable &operator=(SubscriptionPollable &&) = delete;
-
-	/**
-	 * Create an update function that uses the embedded struct.
-	 */
-	bool update() override
-	{
-		return SubscriptionPollableBase::update((void *)(&_data));
-	}
-
-	bool forcedUpdate() override
-	{
-		return orb_copy(_meta, _handle, &_data) == PX4_OK;
-	}
-
-	/*
-	 * This function gets the T struct data
-	 * */
-	const T &get() const
-	{
-		return _data;
-	}
-
-private:
-	T _data;
-};
-
-} // namespace uORB