landing_target_estimator: custom irlock frame params (#18884)

Signed-off-by: ruimscarvalho98 <ruimsc98@gmail.com>

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -47,7 +47,6 @@
 
 #define SEC2USEC 1000000.0f
 
-
 namespace landing_target_estimator
 {
 
@@ -60,7 +59,10 @@ LandingTargetEstimator::LandingTargetEstimator()
 	_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);
 }
 
@@ -115,15 +117,16 @@ void LandingTargetEstimator::update()
 		return;
 	}
 
-	// TODO account for sensor orientation as set by parameter
-	// default orientation has camera x pointing in body y, camera y in body -x
-
 	matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
-	sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward
-	sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right
+	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 the unit ray into the navigation frame, assume sensor frame = body frame
+	// 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;
@@ -133,12 +136,16 @@ void LandingTargetEstimator::update()
 		return;
 	}
 
-	float dist = _vehicleLocalPosition.dist_bottom;
+	float dist = _vehicleLocalPosition.dist_bottom - _params.offset_z;
 
 	// scale the ray s.t. the z component has length of dist
 	_rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
 	_rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
 
+	// Adjust relative position according to sensor offset
+	_rel_pos(0) += _params.offset_x;
+	_rel_pos(1) += _params.offset_y;
+
 	if (!_estimator_initialized) {
 		PX4_INFO("Init");
 		float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
@@ -255,6 +262,14 @@ void LandingTargetEstimator::_update_params()
 
 	param_get(_paramHandle.scale_x, &_params.scale_x);
 	param_get(_paramHandle.scale_y, &_params.scale_y);
+
+	int32_t sensor_yaw = 0;
+	param_get(_paramHandle.sensor_yaw, &sensor_yaw);
+	_params.sensor_yaw = static_cast<enum Rotation>(sensor_yaw);
+
+	param_get(_paramHandle.offset_x, &_params.offset_x);
+	param_get(_paramHandle.offset_y, &_params.offset_y);
+	param_get(_paramHandle.offset_z, &_params.offset_z);
 }
 
 

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -58,6 +58,7 @@
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <matrix/Matrix.hpp>
+#include <lib/conversion/rotation.h>
 #include "KalmanFilter.h"
 
 using namespace time_literals;
@@ -118,6 +119,10 @@ private:
 		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 {
@@ -128,6 +133,10 @@ private:
 		TargetMode mode;
 		float scale_x;
 		float scale_y;
+		float offset_x;
+		float offset_y;
+		float offset_z;
+		enum Rotation sensor_yaw;
 	} _params;
 
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
@@ -149,7 +158,8 @@ private:
 	// keep track of whether last measurement was rejected
 	bool _faulty{false};
 
-	matrix::Dcmf _R_att;
+	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;

src/modules/landing_target_estimator/landing_target_estimator_params.c

@@ -132,3 +132,58 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f);
  * @group Landing target Estimator
  */
 PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f);
+
+
+/**
+ * Rotation of IRLOCK sensor relative to airframe
+ *
+ * Default orientation of Yaw 90°
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ *
+ * @min -1
+ * @max 40
+ * @reboot_required true
+ * @group Landing Target Estimator
+ */
+PARAM_DEFINE_INT32(LTEST_SENS_ROT, 2);
+
+/**
+ * X Position of IRLOCK in body frame (forward)
+ *
+ * @reboot_required true
+ * @unit m
+ * @decimal 3
+ * @group Landing Target Estimator
+ *
+ */
+PARAM_DEFINE_FLOAT(LTEST_SENS_POS_X, 0.0f);
+
+/**
+ * Y Position of IRLOCK in body frame (right)
+ *
+ * @reboot_required true
+ * @unit m
+ * @decimal 3
+ * @group Landing Target Estimator
+ *
+ */
+PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Y, 0.0f);
+
+/**
+ * Z Position of IRLOCK in body frame (downward)
+ *
+ * @reboot_required true
+ * @unit m
+ * @decimal 3
+ * @group Landing Target Estimator
+ *
+ */
+PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Z, 0.0f);