Added yaw tracking to LandingTargetEstimator and precland

ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock

@@ -34,5 +34,10 @@ param set-default LTEST_MODE 1
 param set-default PLD_HACC_RAD 0.1
 param set-default RTL_PLD_MD 2
 
+# Orientation parameters for Gazebo irislock sim
+param set-default LTEST_EULR_PHI 0      	# ENU -> NED already done in gazeebo
+param set-default LTEST_EULR_THETA -1.57	# Camera tilt, which is facing 90 degrees down
+param set-default LTEST_EULR_PSI 0.0
+
 # Start up Landing Target Estimator module
 landing_target_estimator start

msg/IrlockReport.msg

@@ -9,3 +9,5 @@ float32 pos_x # tan(theta), where theta is the angle between the target and the
 float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis
 float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/
 float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/
+
+float32[4] q   # Quaternion as recorded by the camera (w, x, y, z order, zero-rotation is 1, 0, 0, 0)

msg/LandingTargetPose.msg

@@ -24,3 +24,7 @@ bool abs_pos_valid		# Flag showing whether absolute position is valid
 float32 x_abs			# X/north position of target, relative to origin (navigation frame) [meters]
 float32 y_abs			# Y/east position of target, relative to origin (navigation frame) [meters]
 float32 z_abs			# Z/down position of target, relative to origin (navigation frame) [meters]
+float32[4] q		 	# Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)
+
+float32 target_yaw
+float32 target_yaw_filtered

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -63,7 +63,13 @@ LandingTargetEstimator::LandingTargetEstimator()
 	_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");
+	_paramHandle.eulr_phi = param_find("LTEST_EULR_PHI");
+	_paramHandle.eulr_theta = param_find("LTEST_EULR_THETA");
+	_paramHandle.eulr_psi = param_find("LTEST_EULR_PSI");
+	_paramHandle.yaw_alpha = param_find("LTEST_YAW_ALPHA");
+
 	_check_params(true);
+	_alpha_filter_yaw.reset(NAN);
 }
 
 void LandingTargetEstimator::update()
@@ -75,7 +81,7 @@ void LandingTargetEstimator::update()
 	/* predict */
 	if (_estimator_initialized) {
 		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
-			PX4_WARN("Timeout");
+			PX4_INFO("Target lost");
 			_estimator_initialized = false;
 
 		} else {
@@ -116,6 +122,9 @@ void LandingTargetEstimator::update()
 		_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);
 
+		const float sample_interval = 1.0f;
+		_alpha_filter_yaw.setParameters(sample_interval, 0.f);
+
 		_estimator_initialized = true;
 		_last_update = hrt_absolute_time();
 		_last_predict = _last_update;
@@ -126,10 +135,13 @@ void LandingTargetEstimator::update()
 		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);
 
+		_alpha_filter_yaw.setAlpha(_params.yaw_alpha);
+		_alpha_filter_yaw.update(_last_unwrapped_yaw);
+
 		if (!update_x || !update_y) {
 			if (!_faulty) {
 				_faulty = true;
-				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
+				PX4_INFO("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
 			}
 
 		} else {
@@ -170,6 +182,14 @@ void LandingTargetEstimator::update()
 				_target_pose.z_abs = _target_position_report.rel_pos_z  + _vehicleLocalPosition.z;
 				_target_pose.abs_pos_valid = true;
 
+				// q should only be filled when abs_pos_valid is set
+				const float yaw_filterd_and_wrapped = matrix::wrap_pi(_alpha_filter_yaw.getState());
+				matrix::Quatf quaternion(matrix::Eulerf(0.f, 0.f, yaw_filterd_and_wrapped));
+				_target_pose.target_yaw_filtered = yaw_filterd_and_wrapped;
+				_target_pose.target_yaw = _last_unwrapped_yaw;
+				quaternion.copyTo(_target_pose.q);
+				_last_unwrapped_yaw = _target_pose.target_yaw;
+
 			} else {
 				_target_pose.abs_pos_valid = false;
 			}
@@ -242,6 +262,24 @@ void LandingTargetEstimator::_update_topics()
 			return;
 		}
 
+		// Calculate target yaw [rad] in NED frame from offboard quaternion
+		const matrix::Quatf q_in_camera(_irlockReport.q);
+		const matrix::Quatf rot_camera_to_body = matrix::Eulerf(_params.eulr_phi, _params.eulr_theta, _params.eulr_psi);
+		const matrix::Quatf rot_body_to_ned = q_att;
+		const matrix::Quatf q_in_body = rot_camera_to_body * q_in_camera;
+		const matrix::Quatf q_in_ned = rot_body_to_ned * q_in_body;
+		const float target_yaw_ned = matrix::Eulerf(q_in_ned).psi();
+
+		if (PX4_ISFINITE(target_yaw_ned)) {
+			// _last_unwrapped_yaw = unwrap(_last_unwrapped_yaw, target_yaw_ned, 0.0f, (float)M_PI);
+			_last_unwrapped_yaw = matrix::unwrap_pi(_last_unwrapped_yaw, target_yaw_ned);
+
+			// Initialize yaw lowpass filter if necessary
+			if (!PX4_ISFINITE(_alpha_filter_yaw.getState())) {
+				_alpha_filter_yaw.reset(_last_unwrapped_yaw);
+			}
+		}
+
 		_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
 
 		// scale the ray s.t. the z component has length of _uncertainty_scale
@@ -302,6 +340,10 @@ void LandingTargetEstimator::_update_params()
 	param_get(_paramHandle.offset_x, &_params.offset_x);
 	param_get(_paramHandle.offset_y, &_params.offset_y);
 	param_get(_paramHandle.offset_z, &_params.offset_z);
+	param_get(_paramHandle.eulr_phi, &_params.eulr_phi);
+	param_get(_paramHandle.eulr_theta, &_params.eulr_theta);
+	param_get(_paramHandle.eulr_psi, &_params.eulr_psi);
+	param_get(_paramHandle.yaw_alpha, &_params.yaw_alpha);
 }
 
 

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -62,6 +62,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/Matrix.hpp>
 #include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include "KalmanFilter.h"
 
 using namespace time_literals;
@@ -126,6 +127,10 @@ private:
 		param_t offset_y;
 		param_t offset_z;
 		param_t sensor_yaw;
+		param_t eulr_phi;
+		param_t eulr_theta;
+		param_t eulr_psi;
+		param_t yaw_alpha;
 	} _paramHandle;
 
 	struct {
@@ -140,6 +145,10 @@ private:
 		float offset_y;
 		float offset_z;
 		enum Rotation sensor_yaw;
+		float eulr_phi;
+		float eulr_theta;
+		float eulr_psi;
+		float yaw_alpha;
 	} _params;
 
 	struct {
@@ -147,6 +156,7 @@ private:
 		float rel_pos_x;
 		float rel_pos_y;
 		float rel_pos_z;
+		float quaternion[4];
 	} _target_position_report;
 
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
@@ -177,6 +187,8 @@ private:
 	matrix::Vector2f _rel_pos;
 	KalmanFilter _kalman_filter_x;
 	KalmanFilter _kalman_filter_y;
+	AlphaFilter<float> _alpha_filter_yaw; // poor man's orientation estimator
+	float _last_unwrapped_yaw{0.0f};
 	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};

src/modules/landing_target_estimator/landing_target_estimator_params.c

@@ -187,3 +187,51 @@ PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Y, 0.0f);
  *
  */
 PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Z, 0.0f);
+
+/**
+ * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Phi (X)
+ *
+ * Orientation transformation from camera frame that detected
+ * the landing_target to drone's NED frame
+ *
+ * @unit rad
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_EULR_PHI, 0.0f);
+
+/**
+ * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Theta (Y)
+ *
+ * Orientation transformation from camera frame that detected
+ * the landing_target to drone's NED frame
+ *
+ * @unit rad
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_EULR_THETA, 0.0f);
+
+/**
+ * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Psi (Z)
+ *
+ * Orientation transformation from camera frame that detected
+ * the landing_target to drone's NED frame
+ *
+ * @unit rad
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_EULR_PSI, -1.57f);
+
+/**
+ * Target yaw filter alpha value
+ *
+ * @min 0
+ * @max 100
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_YAW_ALPHA, 0.05);

src/modules/mavlink/mavlink_receiver.cpp

@@ -2515,6 +2515,14 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
 				      "landing target: unsupported coordinate frame {1}", landing_target.frame);
 
 	} else {
+		// The following was always a work-around:
+		// irlock_report message, packaged as landing_target message over mavlink
+		// See https://github.com/PX4/PX4-Autopilot/issues/14698
+		//
+		// irlock_report needs to be reworked into a message that supports feeding PX4's
+		// landing_target_estimator from offboard. This should not be limited to
+		// single-point IR beacons, but should support and form of marker, including tags
+		// that have an orientation.
 		irlock_report_s irlock_report{};
 
 		irlock_report.timestamp = hrt_absolute_time();
@@ -2524,6 +2532,10 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
 		irlock_report.size_x = landing_target.size_x;
 		irlock_report.size_y = landing_target.size_y;
 
+		// Copy quaternion
+		matrix::Quatf q(landing_target.q);
+		q.copyTo(irlock_report.q);
+
 		_irlock_report_pub.publish(irlock_report);
 	}
 }

src/modules/navigator/precland.cpp

@@ -268,6 +268,10 @@ PrecLand::run_state_horizontal_approach()
 	pos_sp_triplet->current.alt = _approach_alt;
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 
+	// note: abs_pos_valid was already checked as a condition to enter this state
+	pos_sp_triplet->current.yaw = matrix::Eulerf(matrix::Quaternionf(_target_pose.q)).psi();
+	pos_sp_triplet->current.yaw_valid = true;
+
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -299,6 +303,9 @@ PrecLand::run_state_descend_above_target()
 
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
 
+	pos_sp_triplet->current.yaw = matrix::Eulerf(matrix::Quaternionf(_target_pose.q)).psi();
+	pos_sp_triplet->current.yaw_valid = true;
+
 	_navigator->set_position_setpoint_triplet_updated();
 }
 

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -619,6 +619,9 @@ void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *ms
 		report.size_x = landing_target_mavlink.size_x;
 		report.size_y = landing_target_mavlink.size_y;
 
+		matrix::Quatf q(landing_target_mavlink.q);
+		q.copyTo(report.q);
+
 		_irlock_report_pub.publish(report);
 	}
 }