cleanup landing_target_estimator

msg/landing_target_innovations.msg

@@ -1,4 +1,6 @@
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
 # Innovation of landing target position estimator
 float32 innov_x
 float32 innov_y

msg/landing_target_pose.msg

@@ -1,6 +1,7 @@
 # Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 bool is_static			# Flag indicating whether the landing target is static or moving with respect to the ground
 

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -568,6 +568,9 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
 	return L_inv.T() * L_inv;
 }
 
+using Matrix2f = SquareMatrix<float, 2>;
+using Matrix2d = SquareMatrix<double, 2>;
+
 using Matrix3f = SquareMatrix<float, 3>;
 using Matrix3d = SquareMatrix<double, 3>;
 

src/modules/ekf2/EKF2.cpp

@@ -1515,7 +1515,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 		if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
 			// velocity of vehicle relative to target has opposite sign to target relative to vehicle
 			auxVelSample auxvel_sample{
-				.time_us = landing_target_pose.timestamp,
+				.time_us = landing_target_pose.timestamp_sample,
 				.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
 				.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
 			};

src/modules/landing_target_estimator/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-2022 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
@@ -36,9 +36,9 @@ px4_add_module(
 	MAIN landing_target_estimator
 	COMPILE_FLAGS
 	SRCS
-		landing_target_estimator_main.cpp
-		LandingTargetEstimator.cpp
 		KalmanFilter.cpp
-	DEPENDS
+		KalmanFilter.hpp
+		LandingTargetEstimator.cpp
+		LandingTargetEstimator.hpp
 	)
 

src/modules/landing_target_estimator/KalmanFilter.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -39,16 +39,14 @@
  *
  */
 
-#include "KalmanFilter.h"
+#include "KalmanFilter.hpp"
 
-namespace landing_target_estimator
-{
-KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
+KalmanFilter::KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit)
 {
 	init(initial, covInit);
 }
 
-void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
+void KalmanFilter::init(matrix::Vector2f &initial, matrix::Matrix2f &covInit)
 {
 	_x = initial;
 	_covariance = covInit;
@@ -56,10 +54,10 @@ void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float,
 
 void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
 {
-	matrix::Vector<float, 2> initial;
+	matrix::Vector2f initial;
 	initial(0) = initial0;
 	initial(1) = initial1;
-	matrix::Matrix<float, 2, 2> covInit;
+	matrix::Matrix2f covInit;
 	covInit(0, 0) = covInit00;
 	covInit(1, 1) = covInit11;
 
@@ -71,7 +69,7 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc)
 	_x(0) += _x(1) * dt + dt * dt / 2 * acc;
 	_x(1) += acc * dt;
 
-	matrix::Matrix<float, 2, 2> A; // propagation matrix
+	matrix::Matrix2f A; // propagation matrix
 	A(0, 0) = 1;
 	A(1, 1) = 1;
 	A(0, 1) = dt;
@@ -80,14 +78,13 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc)
 	G(0, 0) = dt * dt / 2;
 	G(1, 0) = dt;
 
-	matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
+	matrix::Matrix2f process_noise = G * G.transpose() * acc_unc;
 
 	_covariance = A * _covariance * A.transpose() + process_noise;
 }
 
 bool KalmanFilter::update(float meas, float measUnc)
 {
-
 	// H = [1, 0]
 	_residual = meas - _x(0);
 
@@ -102,46 +99,23 @@ bool KalmanFilter::update(float meas, float measUnc)
 		return false;
 	}
 
-	matrix::Vector<float, 2> kalmanGain;
+	matrix::Vector2f kalmanGain;
 	kalmanGain(0) = _covariance(0, 0);
 	kalmanGain(1) = _covariance(1, 0);
 	kalmanGain /= _innovCov;
 
 	_x += kalmanGain * _residual;
 
-	matrix::Matrix<float, 2, 2> identity;
+	matrix::Matrix2f identity;
 	identity.identity();
 
-	matrix::Matrix<float, 2, 2> KH; // kalmanGain * H
+	matrix::Matrix2f KH; // kalmanGain * H
 	KH(0, 0) = kalmanGain(0);
 	KH(1, 0) = kalmanGain(1);
 
 	_covariance = (identity - KH) * _covariance;
 
 	return true;
-
-}
-
-void KalmanFilter::getState(matrix::Vector<float, 2> &state)
-{
-	state = _x;
-}
-
-void KalmanFilter::getState(float &state0, float &state1)
-{
-	state0 = _x(0);
-	state1 = _x(1);
-}
-
-void KalmanFilter::getCovariance(matrix::Matrix<float, 2, 2> &covariance)
-{
-	covariance = _covariance;
-}
-
-void KalmanFilter::getCovariance(float &cov00, float &cov11)
-{
-	cov00 = _covariance(0, 0);
-	cov11 = _covariance(1, 1);
 }
 
 void KalmanFilter::getInnovations(float &innov, float &innovCov)
@@ -149,5 +123,3 @@ void KalmanFilter::getInnovations(float &innov, float &innovCov)
 	innov = _residual;
 	innovCov = _innovCov;
 }
-
-} // namespace landing_target_estimator

src/modules/landing_target_estimator/KalmanFilter.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -53,32 +53,30 @@
 
 #pragma once
 
-namespace landing_target_estimator
-{
 class KalmanFilter
 {
 public:
 	/**
 	 * Default constructor, state not initialized
 	 */
-	KalmanFilter() {};
+	KalmanFilter() = default;
 
 	/**
 	 * Constructor, initialize state
 	 */
-	KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
+	KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit);
 
 	/**
 	 * Default desctructor
 	 */
-	virtual ~KalmanFilter() {};
+	~KalmanFilter() = default;
 
 	/**
 	 * Initialize filter state
 	 * @param initial initial state
 	 * @param covInit initial covariance
 	 */
-	void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
+	void init(matrix::Vector2f &initial, matrix::Matrix2f &covInit);
 
 	/**
 	 * Initialize filter state, only specifying diagonal covariance elements
@@ -109,27 +107,13 @@ public:
 	 * Get the current filter state
 	 * @param x1 State
 	 */
-	void getState(matrix::Vector<float, 2> &state);
-
-	/**
-	 * Get the current filter state
-	 * @param state0 First state
-	 * @param state1 Second state
-	 */
-	void getState(float &state0, float &state1);
+	const matrix::Vector2f &getState() const { return _x; }
 
 	/**
 	 * Get state covariance
 	 * @param covariance Covariance of the state
 	 */
-	void getCovariance(matrix::Matrix<float, 2, 2> &covariance);
-
-	/**
-	 * Get state variances (diagonal elements)
-	 * @param cov00 Variance of first state
-	 * @param cov11 Variance of second state
-	 */
-	void getCovariance(float &cov00, float &cov11);
+	const matrix::Matrix2f &getCovariance() const { return _covariance; }
 
 	/**
 	 * Get measurement innovation and covariance of last update call
@@ -139,12 +123,10 @@ public:
 	void getInnovations(float &innov, float &innovCov);
 
 private:
-	matrix::Vector<float, 2> _x; // state
+	matrix::Vector2f _x{}; // state
+	matrix::Matrix2f _covariance{}; // state covariance
 
-	matrix::Matrix<float, 2, 2> _covariance; // state covariance
+	float _residual{0.f}; // residual of last measurement update
 
-	float _residual{0.0f}; // residual of last measurement update
-
-	float _innovCov{0.0f}; // innovation covariance of last measurement update
+	float _innovCov{0.f}; // innovation covariance of last measurement update
 };
-} // namespace landing_target_estimator

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -31,279 +31,307 @@
  *
  ****************************************************************************/
 
-/*
- * @file LandingTargetEstimator.cpp
- *
- * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
- * @author Mohammed Kabir <kabir@uasys.io>
- *
- */
+#include "LandingTargetEstimator.hpp"
 
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <drivers/drv_hrt.h>
-
-#include "LandingTargetEstimator.h"
-
-#define SEC2USEC 1000000.0f
-
-namespace landing_target_estimator
+LandingTargetEstimator::LandingTargetEstimator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
-
-LandingTargetEstimator::LandingTargetEstimator()
-{
-	_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
-	_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
-	_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
-	_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
-	_paramHandle.mode = param_find("LTEST_MODE");
-	_paramHandle.scale_x = param_find("LTEST_SCALE_X");
-	_paramHandle.scale_y = param_find("LTEST_SCALE_Y");
-	_paramHandle.sensor_yaw = param_find("LTEST_SENS_ROT");
-	_paramHandle.offset_x = param_find("LTEST_SENS_POS_X");
-	_paramHandle.offset_y = param_find("LTEST_SENS_POS_Y");
-	_paramHandle.offset_z = param_find("LTEST_SENS_POS_Z");
-	_check_params(true);
 }
 
-void LandingTargetEstimator::update()
+LandingTargetEstimator::~LandingTargetEstimator()
 {
-	_check_params(false);
+	perf_free(_cycle_perf);
+}
 
-	_update_topics();
-
-	/* predict */
-	if (_estimator_initialized) {
-		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
-			PX4_WARN("Timeout");
-			_estimator_initialized = false;
-
-		} else {
-			float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
-
-			// predict target position with the help of accel data
-			matrix::Vector3f a{_vehicle_acceleration.xyz};
-
-			if (_vehicleAttitude_valid && _vehicle_acceleration_valid) {
-				matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
-				_R_att = matrix::Dcm<float>(q_att);
-				a = _R_att * a;
-
-			} else {
-				a.zero();
-			}
-
-			_kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
-			_kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
-
-			_last_predict = hrt_absolute_time();
-		}
+bool LandingTargetEstimator::init()
+{
+	if (!_vehicle_local_position_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
 	}
 
-	if (!_new_sensorReport) {
-		// nothing to do
+	return true;
+}
+
+void LandingTargetEstimator::Run()
+{
+	if (should_exit()) {
+		_vehicle_local_position_sub.unregisterCallback();
+		exit_and_cleanup();
 		return;
 	}
 
-	// mark this sensor measurement as consumed
-	_new_sensorReport = false;
+	// backup schedule
+	ScheduleDelayed(100_ms);
 
-
-	if (!_estimator_initialized) {
-		float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
-		float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
-		PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
-		_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init);
-		_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init);
-
-		_estimator_initialized = true;
-		_last_update = hrt_absolute_time();
-		_last_predict = _last_update;
-
-	} else {
-		// update
-		const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z;
-		bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
-		bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
-
-		if (!update_x || !update_y) {
-			if (!_faulty) {
-				_faulty = true;
-				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
-			}
-
-		} else {
-			_faulty = false;
-		}
-
-		if (!_faulty) {
-			// only publish if both measurements were good
-
-			_target_pose.timestamp = _target_position_report.timestamp;
-
-			float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
-			_kalman_filter_x.getState(x, xvel);
-			_kalman_filter_x.getCovariance(covx, covx_v);
-
-			_kalman_filter_y.getState(y, yvel);
-			_kalman_filter_y.getCovariance(covy, covy_v);
-
-			_target_pose.is_static = (_params.mode == TargetMode::Stationary);
-
-			_target_pose.rel_pos_valid = true;
-			_target_pose.rel_vel_valid = true;
-			_target_pose.x_rel = x;
-			_target_pose.y_rel = y;
-			_target_pose.z_rel = _target_position_report.rel_pos_z ;
-			_target_pose.vx_rel = xvel;
-			_target_pose.vy_rel = yvel;
-
-			_target_pose.cov_x_rel = covx;
-			_target_pose.cov_y_rel = covy;
-
-			_target_pose.cov_vx_rel = covx_v;
-			_target_pose.cov_vy_rel = covy_v;
-
-			if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
-				_target_pose.x_abs = x + _vehicleLocalPosition.x;
-				_target_pose.y_abs = y + _vehicleLocalPosition.y;
-				_target_pose.z_abs = _target_position_report.rel_pos_z  + _vehicleLocalPosition.z;
-				_target_pose.abs_pos_valid = true;
-
-			} else {
-				_target_pose.abs_pos_valid = false;
-			}
-
-			_targetPosePub.publish(_target_pose);
-
-			_last_update = hrt_absolute_time();
-			_last_predict = _last_update;
-		}
-
-		float innov_x, innov_cov_x, innov_y, innov_cov_y;
-		_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
-		_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
-
-		_target_innovations.timestamp = _target_position_report.timestamp;
-		_target_innovations.innov_x = innov_x;
-		_target_innovations.innov_cov_x = innov_cov_x;
-		_target_innovations.innov_y = innov_y;
-		_target_innovations.innov_cov_y = innov_cov_y;
-
-		_targetInnovationsPub.publish(_target_innovations);
-	}
-}
-
-void LandingTargetEstimator::_check_params(const bool force)
-{
-	if (_parameter_update_sub.updated() || force) {
+	if (_parameter_update_sub.updated()) {
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);
 
-		_update_params();
+		ModuleParams::updateParams();
 	}
-}
 
-void LandingTargetEstimator::_update_topics()
-{
-	_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
-	_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
-	_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
+	perf_begin(_cycle_perf);
 
+	// predict
+	vehicle_local_position_s vehicle_local_position;
 
-	if (_irlockReportSub.update(&_irlockReport)) { //
-		_new_irlockReport = true;
+	if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
+		if (_estimator_initialized) {
+			const float dt = math::constrain((vehicle_local_position.timestamp_sample - _last_predict) * 1e-6f, 0.001f, 0.1f);
 
-		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
-			// don't have the data needed for an update
-			return;
+			// predict target position with the help of accel data
+			_kalman_filter_x.predict(dt, -vehicle_local_position.ax, _param_ltest_acc_unc.get());
+			_kalman_filter_y.predict(dt, -vehicle_local_position.ay, _param_ltest_acc_unc.get());
+
+			_last_predict = vehicle_local_position.timestamp_sample;
 		}
-
-		if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
-			return;
-		}
-
-		matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
-		sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
-		sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
-		sensor_ray(2) = 1.0f;
-
-		// rotate unit ray according to sensor orientation
-		_S_att = get_rot_matrix(_params.sensor_yaw);
-		sensor_ray = _S_att * sensor_ray;
-
-		// rotate the unit ray into the navigation frame
-		matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
-		_R_att = matrix::Dcm<float>(q_att);
-		sensor_ray = _R_att * sensor_ray;
-
-		if (fabsf(sensor_ray(2)) < 1e-6f) {
-			// z component of measurement unsafe, don't use this measurement
-			return;
-		}
-
-		_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
-
-		// scale the ray s.t. the z component has length of _uncertainty_scale
-		_target_position_report.timestamp = _irlockReport.timestamp;
-		_target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z;
-		_target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z;
-		_target_position_report.rel_pos_z = _dist_z;
-
-		// Adjust relative position according to sensor offset
-		_target_position_report.rel_pos_x += _params.offset_x;
-		_target_position_report.rel_pos_y += _params.offset_y;
-
-		_new_sensorReport = true;
-
-	} else if (_uwbDistanceSub.update(&_uwbDistance)) {
-		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
-			// don't have the data needed for an update
-			PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
-			return;
-		}
-
-		if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) ||
-		    !PX4_ISFINITE((float)_uwbDistance.position[2])) {
-			PX4_WARN("Position is corrupt!");
-			return;
-		}
-
-		_new_sensorReport = true;
-
-		// The coordinate system is NED (north-east-down)
-		// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
-		// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
-		// To change POV we negate every Axis:
-		_target_position_report.timestamp = _uwbDistance.timestamp;
-		_target_position_report.rel_pos_x = -_uwbDistance.position[0];
-		_target_position_report.rel_pos_y = -_uwbDistance.position[1];
-		_target_position_report.rel_pos_z = -_uwbDistance.position[2];
 	}
+
+	landing_target_pose_s landing_target_pose{};
+
+	bool update_x = false;
+	bool update_y = false;
+
+	if (_irlock_report_sub.advertised()) {
+		// using irlock_report
+		irlock_report_s irlock_report;
+
+		if (_irlock_report_sub.update(&irlock_report)) {
+			vehicle_attitude_s vehicle_attitude;
+
+			if (!vehicle_local_position.v_xy_valid || !vehicle_local_position.dist_bottom_valid
+			    || !_vehicle_attitude_sub.update(&vehicle_attitude)
+			    || !PX4_ISFINITE(irlock_report.pos_y) || !PX4_ISFINITE(irlock_report.pos_x)) {
+				// don't have the data needed for an update
+				return;
+			}
+
+			matrix::Vector3f sensor_ray; // ray pointing towards target in body frame
+			sensor_ray(0) = irlock_report.pos_x * _param_ltest_scale_x.get(); // forward
+			sensor_ray(1) = irlock_report.pos_y * _param_ltest_scale_y.get(); // right
+			sensor_ray(2) = 1.f;
+
+			// Orientation of the sensor relative to body frame
+			const matrix::Dcmf S_att = get_rot_matrix((Rotation)_param_ltest_sens_rot.get());
+			// rotate unit ray according to sensor orientation
+			sensor_ray = S_att * sensor_ray;
+
+			// rotate the unit ray into the navigation frame
+			const matrix::Quatf q_att(vehicle_attitude.q);
+			const matrix::Dcmf R_att(q_att); // Orientation of the body frame
+			sensor_ray = R_att * sensor_ray;
+
+			if (fabsf(sensor_ray(2)) < 1e-6f) {
+				// z component of measurement unsafe, don't use this measurement
+				return;
+			}
+
+			matrix::Vector3f rel_pos{};
+
+			const float dist_z = vehicle_local_position.dist_bottom - _param_ltest_sens_pos_z.get();
+
+			// scale the ray s.t. the z component has length of _uncertainty_scale
+			//  - adjust relative position according to sensor offset
+			rel_pos(0) = (sensor_ray(0) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get();
+			rel_pos(1) = (sensor_ray(1) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get();
+			rel_pos(2) = dist_z;
+
+			if (!_estimator_initialized) {
+				float vx_init = -vehicle_local_position.vx;
+				float vy_init = -vehicle_local_position.vy;
+
+				PX4_INFO("IRLock target acquired %.2f %.2f", (double)vx_init, (double)vy_init);
+
+				_kalman_filter_x.init(rel_pos(0), vx_init, _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
+				_kalman_filter_y.init(rel_pos(1), vy_init,  _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
+
+				_estimator_initialized = true;
+				_last_predict = vehicle_local_position.timestamp_sample;
+				_last_update = vehicle_local_position.timestamp_sample;
+
+			} else {
+				const float measurement_uncertainty = _param_ltest_meas_unc.get() * dist_z * dist_z;
+
+				update_x = _kalman_filter_x.update(rel_pos(0), measurement_uncertainty);
+				update_y = _kalman_filter_y.update(rel_pos(1), measurement_uncertainty);
+
+				if (!update_x || !update_y) {
+					PX4_DEBUG("IRLock distance measurement rejected (%.3f, %.3f)", (double)rel_pos(0), (double)rel_pos(1));
+				}
+
+				landing_target_pose.timestamp_sample = irlock_report.timestamp;
+				landing_target_pose.z_rel = dist_z;
+			}
+		}
+
+	} else if (_uwb_distance_sub.advertised()) {
+		uwb_distance_s uwb_distance;
+
+		if (_uwb_distance_sub.update(&uwb_distance)) {
+
+			if (!vehicle_local_position.v_xy_valid) {
+				// don't have the data needed for an update
+				return;
+			}
+
+			if (!PX4_ISFINITE((float)uwb_distance.position[0])
+			    || !PX4_ISFINITE((float)uwb_distance.position[1])
+			    || !PX4_ISFINITE((float)uwb_distance.position[2])) {
+
+				PX4_WARN("uwb_distance position is corrupt!");
+				return;
+			}
+
+			const matrix::Vector3f position(-uwb_distance.position[0], -uwb_distance.position[1], -uwb_distance.position[2]);
+
+			// The coordinate system is NED (north-east-down)
+			// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
+			// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
+			// To change POV we negate every Axis:
+			if (!_estimator_initialized) {
+				float vx_init = -vehicle_local_position.vx;
+				float vy_init = -vehicle_local_position.vy;
+
+				PX4_INFO("UWB distance target acquired %.2f %.2f", (double)vx_init, (double)vy_init);
+
+				_kalman_filter_x.init(position(0), vx_init,
+						      _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
+
+				_kalman_filter_y.init(position(1), vy_init,
+						      _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get());
+
+				_estimator_initialized = true;
+				_last_predict = vehicle_local_position.timestamp_sample;
+				_last_update = vehicle_local_position.timestamp_sample;
+
+			} else {
+
+				float measurement_uncertainty = _param_ltest_meas_unc.get();
+
+				update_x = _kalman_filter_x.update(position(0), measurement_uncertainty);
+				update_y = _kalman_filter_y.update(position(1), measurement_uncertainty);
+
+				if (!update_x || !update_y) {
+					PX4_DEBUG("UWB distance measurement rejected (%.3f, %.3f)", (double)position(1), (double)position(1));
+				}
+
+				landing_target_pose.timestamp_sample = uwb_distance.timestamp;
+				landing_target_pose.z_rel = uwb_distance.position[2];
+			}
+		}
+	}
+
+	if (vehicle_local_position.timestamp_sample > _last_update + TARGET_UPDATE_TIMEOUT_US) {
+		if (_estimator_initialized) {
+			PX4_INFO("Target lost");
+			_estimator_initialized = false;
+		}
+	}
+
+	if (_estimator_initialized) {
+		// publish landing_target_pose
+		if (update_x && update_y) {
+			_last_update = vehicle_local_position.timestamp_sample;
+
+			// only publish if both measurements were good
+			landing_target_pose.is_static = ((TargetMode)_param_ltest_mode.get() == TargetMode::Stationary);
+
+			landing_target_pose.rel_pos_valid = true;
+			landing_target_pose.rel_vel_valid = true;
+
+			landing_target_pose.x_rel      = _kalman_filter_x.getState()(0);
+			landing_target_pose.vx_rel     = _kalman_filter_x.getState()(1);
+			landing_target_pose.cov_x_rel  = _kalman_filter_x.getCovariance()(0, 0);
+			landing_target_pose.cov_vx_rel = _kalman_filter_x.getCovariance()(1, 1);
+
+			landing_target_pose.y_rel      = _kalman_filter_y.getState()(0);
+			landing_target_pose.vy_rel     = _kalman_filter_y.getState()(1);
+			landing_target_pose.cov_y_rel  = _kalman_filter_y.getCovariance()(0, 0);
+			landing_target_pose.cov_vy_rel = _kalman_filter_y.getCovariance()(1, 1);
+
+			if (vehicle_local_position.xy_valid) {
+				landing_target_pose.x_abs = landing_target_pose.x_rel + vehicle_local_position.x;
+				landing_target_pose.y_abs = landing_target_pose.y_rel + vehicle_local_position.y;
+				landing_target_pose.z_abs = landing_target_pose.z_rel + vehicle_local_position.z;
+				landing_target_pose.abs_pos_valid = true;
+
+			} else {
+				landing_target_pose.x_abs = NAN;
+				landing_target_pose.y_abs = NAN;
+				landing_target_pose.z_abs = NAN;
+				landing_target_pose.abs_pos_valid = false;
+			}
+
+			landing_target_pose.timestamp = hrt_absolute_time();
+			_landing_target_pose_pub.publish(landing_target_pose);
+		}
+
+		// publish landing_target_innovations
+		if (landing_target_pose.timestamp_sample != 0) {
+
+			landing_target_innovations_s landing_target_innovations{};
+			_kalman_filter_x.getInnovations(landing_target_innovations.innov_x, landing_target_innovations.innov_cov_x);
+			_kalman_filter_y.getInnovations(landing_target_innovations.innov_y, landing_target_innovations.innov_cov_y);
+
+			landing_target_innovations.timestamp_sample = landing_target_pose.timestamp_sample;
+			landing_target_innovations.timestamp = hrt_absolute_time();
+			_landing_target_innovations_pub.publish(landing_target_innovations);
+		}
+	}
+
+	perf_end(_cycle_perf);
 }
 
-void LandingTargetEstimator::_update_params()
+int LandingTargetEstimator::task_spawn(int argc, char *argv[])
 {
-	param_get(_paramHandle.acc_unc, &_params.acc_unc);
-	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);
+	LandingTargetEstimator *instance = new LandingTargetEstimator();
 
-	int32_t mode = 0;
-	param_get(_paramHandle.mode, &mode);
-	_params.mode = (TargetMode)mode;
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
 
-	param_get(_paramHandle.scale_x, &_params.scale_x);
-	param_get(_paramHandle.scale_y, &_params.scale_y);
+		if (instance->init()) {
+			return PX4_OK;
+		}
 
-	int32_t sensor_yaw = 0;
-	param_get(_paramHandle.sensor_yaw, &sensor_yaw);
-	_params.sensor_yaw = static_cast<enum Rotation>(sensor_yaw);
+	} else {
+		PX4_ERR("alloc failed");
+	}
 
-	param_get(_paramHandle.offset_x, &_params.offset_x);
-	param_get(_paramHandle.offset_y, &_params.offset_y);
-	param_get(_paramHandle.offset_z, &_params.offset_z);
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
 }
 
+int LandingTargetEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
 
-} // namespace landing_target_estimator
+int LandingTargetEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Target position estimator using either IRLock or UWB (ultra wide band) data.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("landing_target_estimator", "estimator");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[])
+{
+	return LandingTargetEstimator::main(argc, argv);
+}

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -1,188 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2018 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 LandingTargetEstimator.h
- * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor.
- *
- * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
- * @author Mohammed Kabir <kabir@uasys.io>
- *
- */
-
-#pragma once
-
-#include <px4_platform_common/workqueue.h>
-#include <drivers/drv_hrt.h>
-#include <parameters/param.h>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/vehicle_acceleration.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/irlock_report.h>
-#include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/landing_target_innovations.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/estimator_sensor_bias.h>
-#include <uORB/topics/parameter_update.h>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <matrix/Matrix.hpp>
-#include <lib/conversion/rotation.h>
-#include "KalmanFilter.h"
-
-using namespace time_literals;
-
-namespace landing_target_estimator
-{
-
-class LandingTargetEstimator
-{
-public:
-
-	LandingTargetEstimator();
-	virtual ~LandingTargetEstimator() = default;
-
-	/*
-	 * Get new measurements and update the state estimate
-	 */
-	void update();
-
-protected:
-
-	/*
-	 * Update uORB topics.
-	 */
-	void _update_topics();
-
-	/*
-	 * Update parameters.
-	 */
-	void _update_params();
-
-	/* timeout after which filter is reset if target not seen */
-	static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
-
-	uORB::Publication<landing_target_pose_s> _targetPosePub{ORB_ID(landing_target_pose)};
-	landing_target_pose_s _target_pose{};
-
-	uORB::Publication<landing_target_innovations_s> _targetInnovationsPub{ORB_ID(landing_target_innovations)};
-	landing_target_innovations_s _target_innovations{};
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-private:
-
-	enum class TargetMode {
-		Moving = 0,
-		Stationary
-	};
-
-	/**
-	* Handles for parameters
-	**/
-	struct {
-		param_t acc_unc;
-		param_t meas_unc;
-		param_t pos_unc_init;
-		param_t vel_unc_init;
-		param_t mode;
-		param_t scale_x;
-		param_t scale_y;
-		param_t offset_x;
-		param_t offset_y;
-		param_t offset_z;
-		param_t sensor_yaw;
-	} _paramHandle;
-
-	struct {
-		float acc_unc;
-		float meas_unc;
-		float pos_unc_init;
-		float vel_unc_init;
-		TargetMode mode;
-		float scale_x;
-		float scale_y;
-		float offset_x;
-		float offset_y;
-		float offset_z;
-		enum Rotation sensor_yaw;
-	} _params;
-
-	struct {
-		hrt_abstime timestamp;
-		float rel_pos_x;
-		float rel_pos_y;
-		float rel_pos_z;
-	} _target_position_report;
-
-	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
-	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
-	uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
-
-	vehicle_local_position_s	_vehicleLocalPosition{};
-	vehicle_attitude_s		_vehicleAttitude{};
-	vehicle_acceleration_s		_vehicle_acceleration{};
-	irlock_report_s			_irlockReport{};
-	uwb_grid_s		_uwbGrid{};
-	uwb_distance_s		_uwbDistance{};
-
-	// keep track of which topics we have received
-	bool _vehicleLocalPosition_valid{false};
-	bool _vehicleAttitude_valid{false};
-	bool _vehicle_acceleration_valid{false};
-	bool _new_irlockReport{false};
-	bool _new_sensorReport{false};
-	bool _estimator_initialized{false};
-	// keep track of whether last measurement was rejected
-	bool _faulty{false};
-
-	matrix::Dcmf _R_att; //Orientation of the body frame
-	matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame
-	matrix::Vector2f _rel_pos;
-	KalmanFilter _kalman_filter_x;
-	KalmanFilter _kalman_filter_y;
-	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
-	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
-	float _dist_z{1.0f};
-
-	void _check_params(const bool force);
-
-	void _update_state();
-};
-} // namespace landing_target_estimator

src/modules/landing_target_estimator/LandingTargetEstimator.hpp

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2022 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.
+ *
+ ****************************************************************************/
+
+
+#pragma once
+
+#include "KalmanFilter.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/irlock_report.h>
+#include <uORB/topics/landing_target_innovations.h>
+#include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/uwb_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+using namespace time_literals;
+
+class LandingTargetEstimator : public ModuleBase<LandingTargetEstimator>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	LandingTargetEstimator();
+	~LandingTargetEstimator() override;
+
+	/** @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:
+	void Run() override;
+
+	enum class TargetMode {
+		Moving = 0,
+		Stationary
+	};
+
+	static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2_s};
+
+	uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
+	uORB::Publication<landing_target_innovations_s> _landing_target_innovations_pub{ORB_ID(landing_target_innovations)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
+
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _irlock_report_sub{ORB_ID(irlock_report)};
+	uORB::Subscription _uwb_distance_sub{ORB_ID(uwb_distance)};
+
+	bool _estimator_initialized{false};
+
+	KalmanFilter _kalman_filter_x{};
+	KalmanFilter _kalman_filter_y{};
+	hrt_abstime _last_predict{0}; // timestamp of last filter prediction
+	hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
+		(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_ltest_acc_unc,
+		(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_ltest_meas_unc,
+		(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_ltest_pos_unc_in,
+		(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_ltest_vel_unc_in,
+		(ParamFloat<px4::params::LTEST_SCALE_X>) _param_ltest_scale_x,
+		(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_ltest_scale_y,
+		(ParamInt<px4::params::LTEST_SENS_ROT>) _param_ltest_sens_rot,
+		(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_ltest_sens_pos_x,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_ltest_sens_pos_y,
+		(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_ltest_sens_pos_z
+	)
+};

src/modules/landing_target_estimator/landing_target_estimator_main.cpp

@@ -1,153 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2018 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 landing_target_estimator_main.cpp
- * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor.
- *
- * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
- * @author Mohammed Kabir <kabir@uasys.io>
- *
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/tasks.h>
-#include <px4_platform_common/posix.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <drivers/drv_hrt.h>
-#include <systemlib/err.h>
-
-#include "LandingTargetEstimator.h"
-
-
-namespace landing_target_estimator
-{
-
-static bool thread_should_exit = false;	/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;			/**< Handle of daemon task / thread */
-
-/* Run main loop at this rate in Hz. */
-static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50;
-
-/**
- * Landing target position estimator app start / stop handling function
- * This makes the module accessible from the nuttx shell
- * @ingroup apps
- */
-extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]);
-
-/**
- * Mainloop of daemon.
- */
-int landing_target_estimator_thread_main(int argc, char *argv[]);
-
-/**
-* Main entry point for this module
-**/
-int landing_target_estimator_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		goto exiterr;
-	}
-
-	if (argc >= 2 && !strcmp(argv[1], "start")) {
-		if (thread_running) {
-			PX4_INFO("already running");
-			/* this is not an error */
-			return 0;
-		}
-
-		thread_should_exit = false;
-		daemon_task = px4_task_spawn_cmd("landing_target_estimator",
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_DEFAULT,
-						 2100,
-						 landing_target_estimator_thread_main,
-						 (argv) ? (char *const *)&argv[2] : nullptr);
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-
-		if (!thread_running) {
-			PX4_WARN("landing_target_estimator not running");
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			PX4_INFO("running");
-
-		} else {
-			PX4_INFO("not started");
-		}
-
-		return 0;
-	}
-
-exiterr:
-	PX4_WARN("usage: landing_target_estimator {start|stop|status}");
-	return 1;
-}
-
-int landing_target_estimator_thread_main(int argc, char *argv[])
-{
-	PX4_DEBUG("starting");
-
-	thread_running = true;
-
-	LandingTargetEstimator est;
-
-	while (!thread_should_exit) {
-		est.update();
-		px4_usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ);
-	}
-
-	PX4_DEBUG("exiting");
-
-	thread_running = false;
-
-	return 0;
-}
-
-}