landing_target_estimator: update to uORB Subscription and Publication

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -51,18 +51,7 @@
 namespace landing_target_estimator
 {
 
-LandingTargetEstimator::LandingTargetEstimator() :
+LandingTargetEstimator::LandingTargetEstimator()
-	_targetPosePub(nullptr),
-	_targetInnovationsPub(nullptr),
-	_paramHandle(),
-	_vehicleLocalPosition_valid(false),
-	_vehicleAttitude_valid(false),
-	_sensorBias_valid(false),
-	_new_irlockReport(false),
-	_estimator_initialized(false),
-	_faulty(false),
-	_last_predict(0),
-	_last_update(0)
 {
 	_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
 	_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
@@ -72,9 +61,6 @@ LandingTargetEstimator::LandingTargetEstimator() :
 	_paramHandle.scale_x = param_find("LTEST_SCALE_X");
 	_paramHandle.scale_y = param_find("LTEST_SCALE_Y");
 
-	// Initialize uORB topics.
-	_initialize_topics();
-
 	_check_params(true);
 }
 
@@ -220,12 +206,7 @@ void LandingTargetEstimator::update()
 				_target_pose.abs_pos_valid = false;
 			}
 
-			if (_targetPosePub == nullptr) {
+			_targetPosePub.publish(_target_pose);
-				_targetPosePub = orb_advertise(ORB_ID(landing_target_pose), &_target_pose);
-
-			} else {
-				orb_publish(ORB_ID(landing_target_pose), _targetPosePub, &_target_pose);
-			}
 
 			_last_update = hrt_absolute_time();
 			_last_predict = _last_update;
@@ -241,25 +222,17 @@ void LandingTargetEstimator::update()
 		_target_innovations.innov_y = innov_y;
 		_target_innovations.innov_cov_y = innov_cov_y;
 
-		if (_targetInnovationsPub == nullptr) {
+		_targetInnovationsPub.publish(_target_innovations);
-			_targetInnovationsPub = orb_advertise(ORB_ID(landing_target_innovations), &_target_innovations);
-
-		} else {
-			orb_publish(ORB_ID(landing_target_innovations), _targetInnovationsPub, &_target_innovations);
 	}
 }
 
-}
-
 void LandingTargetEstimator::_check_params(const bool force)
 {
-	bool updated;
+	bool updated = _parameterSub.updated();
-	parameter_update_s paramUpdate;
-
-	orb_check(_parameterSub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+		parameter_update_s paramUpdate;
+		_parameterSub.copy(&paramUpdate);
 	}
 
 	if (updated || force) {
@@ -267,44 +240,13 @@ void LandingTargetEstimator::_check_params(const bool force)
 	}
 }
 
-void LandingTargetEstimator::_initialize_topics()
-{
-	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
-	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias));
-	_irlockReportSub = orb_subscribe(ORB_ID(irlock_report));
-	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
-}
-
 void LandingTargetEstimator::_update_topics()
 {
-	_vehicleLocalPosition_valid = _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub,
+	_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
-				      &_vehicleLocalPosition);
+	_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
-	_vehicleAttitude_valid = _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
+	_sensorBias_valid = _sensorBiasSub.update(&_sensorBias);
-	_sensorBias_valid = _orb_update(ORB_ID(sensor_bias), _sensorBiasSub, &_sensorBias);
 
-	_new_irlockReport = _orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport);
+	_new_irlockReport = _irlockReportSub.update(&_irlockReport);
-}
-
-
-bool LandingTargetEstimator::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
-{
-	bool newData = false;
-
-	// check if there is new data to grab
-	if (orb_check(handle, &newData) != OK) {
-		return false;
-	}
-
-	if (!newData) {
-		return false;
-	}
-
-	if (orb_copy(meta, handle, buffer) != OK) {
-		return false;
-	}
-
-	return true;
 }
 
 void LandingTargetEstimator::_update_params()
@@ -313,9 +255,11 @@ void LandingTargetEstimator::_update_params()
 	param_get(_paramHandle.meas_unc, &_params.meas_unc);
 	param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init);
 	param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init);
+
 	int32_t mode = 0;
 	param_get(_paramHandle.mode, &mode);
 	_params.mode = (TargetMode)mode;
+
 	param_get(_paramHandle.scale_x, &_params.scale_x);
 	param_get(_paramHandle.scale_y, &_params.scale_y);
 }

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -45,7 +45,8 @@
 #include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
 #include <parameters/param.h>
-#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/sensor_bias.h>
@@ -75,10 +76,6 @@ public:
 	void update();
 
 protected:
-	/*
-	 * Called once to initialize uORB topics.
-	 */
-	void _initialize_topics();
 
 	/*
 	 * Update uORB topics.
@@ -90,23 +87,16 @@ protected:
 	 */
 	void _update_params();
 
-	/*
-	 * Convenience function for polling uORB subscriptions.
-	 *
-	 * @return true if there was new data and it was successfully copied
-	 */
-	static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
-
 	/* timeout after which filter is reset if target not seen */
 	static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
 
-	orb_advert_t _targetPosePub;
+	uORB::Publication<landing_target_pose_s> _targetPosePub{ORB_ID(landing_target_pose)};
-	struct landing_target_pose_s _target_pose;
+	landing_target_pose_s _target_pose{};
 
-	orb_advert_t _targetInnovationsPub;
+	uORB::Publication<landing_target_innovations_s> _targetInnovationsPub{ORB_ID(landing_target_innovations)};
-	struct landing_target_innovations_s _target_innovations;
+	landing_target_innovations_s _target_innovations{};
 
-	int _parameterSub;
+	uORB::Subscription _parameterSub{ORB_ID(parameter_update)};
 
 private:
 
@@ -138,31 +128,31 @@ private:
 		float scale_y;
 	} _params;
 
-	int _vehicleLocalPositionSub;
+	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
-	int _attitudeSub;
+	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
-	int _sensorBiasSub;
+	uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)};
-	int _irlockReportSub;
+	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
 
-	struct vehicle_local_position_s	_vehicleLocalPosition;
+	vehicle_local_position_s	_vehicleLocalPosition{};
-	struct vehicle_attitude_s	_vehicleAttitude;
+	vehicle_attitude_s	_vehicleAttitude{};
-	struct sensor_bias_s		_sensorBias;
+	sensor_bias_s		_sensorBias{};
-	struct irlock_report_s		_irlockReport;
+	irlock_report_s		_irlockReport{};
 
 	// keep track of which topics we have received
-	bool _vehicleLocalPosition_valid;
+	bool _vehicleLocalPosition_valid{false};
-	bool _vehicleAttitude_valid;
+	bool _vehicleAttitude_valid{false};
-	bool _sensorBias_valid;
+	bool _sensorBias_valid{false};
-	bool _new_irlockReport;
+	bool _new_irlockReport{false};
-	bool _estimator_initialized;
+	bool _estimator_initialized{false};
 	// keep track of whether last measurement was rejected
-	bool _faulty;
+	bool _faulty{false};
 
-	matrix::Dcm<float> _R_att;
+	matrix::Dcmf _R_att;
-	matrix::Vector<float, 2> _rel_pos;
+	matrix::Vector2f _rel_pos;
 	KalmanFilter _kalman_filter_x;
 	KalmanFilter _kalman_filter_y;
-	hrt_abstime _last_predict; // timestamp of last filter prediction
+	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
-	hrt_abstime _last_update; // timestamp of last filter update (used to check timeout)
+	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
 
 	void _check_params(const bool force);