landing_target_estimator: Sensor offset from pr-nxp-uwb-driver

CopterExpress · Dec 3, 2019 · ca61007 · ca61007
1 parent 8b9e52b
commit ca61007
Show file tree

Hide file tree

Showing 3 changed files with 71 additions and 2 deletions.
diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp
@@ -71,8 +71,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.offset_x = param_find("LTEST_OFFSET_X");
-	_paramHandle.offset_y = param_find("LTEST_OFFSET_Y");
+	_paramHandle.offset_x = param_find("LTEST_OFF_X");
+	_paramHandle.offset_y = param_find("LTEST_OFF_Y");
+	_paramHandle.sensor_offset_x = param_find("LTEST_S_OFF_X");
+	_paramHandle.sensor_offset_y = param_find("LTEST_S_OFF_Y");
 
 	// Initialize uORB topics.
 	_initialize_topics();
@@ -198,6 +200,19 @@ void LandingTargetEstimator::update()
 
 			_target_pose.is_static = (_params.mode == TargetMode::Stationary);
 
+			// offset the landing target in earth frame
+			x += _params.offset_x;
+			y += _params.offset_y;
+
+			// offset the landing target in body frame
+			matrix::Vector3f off;
+			off(0) = _params.sensor_offset_x;
+			off(1) = _params.sensor_offset_y;
+			off(2) = 0.0f;
+			off = _R_att * off;
+			x += off(0);
+			y += off(1);
+
 			_target_pose.rel_pos_valid = true;
 			_target_pose.rel_vel_valid = true;
 			_target_pose.x_rel = x;
@@ -322,6 +337,8 @@ void LandingTargetEstimator::_update_params()
 	param_get(_paramHandle.scale_y, &_params.scale_y);
 	param_get(_paramHandle.offset_x, &_params.offset_x);
 	param_get(_paramHandle.offset_y, &_params.offset_y);
+	param_get(_paramHandle.sensor_offset_x, &_params.sensor_offset_x);
+	param_get(_paramHandle.sensor_offset_y, &_params.sensor_offset_y);
 }
 
 

diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h
@@ -128,6 +128,8 @@ class LandingTargetEstimator
 		param_t scale_y;
 		param_t offset_x;
 		param_t offset_y;
+		param_t sensor_offset_x;
+		param_t sensor_offset_y;
 	} _paramHandle;
 
 	struct {
@@ -140,6 +142,8 @@ class LandingTargetEstimator
 		float scale_y;
 		float offset_x;
 		float offset_y;
+		float sensor_offset_x;
+		float sensor_offset_y;
 	} _params;
 
 	int _vehicleLocalPositionSub;

diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c
@@ -173,3 +173,51 @@ PARAM_DEFINE_FLOAT(LTEST_OFFSET_Y, 0.0f);
  * @value 2 IR-Lock
  */
 PARAM_DEFINE_INT32(LTEST_EN, 0);
+
+/**
+ * Landing target X offset from estimated position in NED
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_OFF_X, 0.0f);
+
+/**
+ * Landing target Y offset from estimated position in NED
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_OFF_Y, 0.0f);
+
+/**
+ * Sensor X position offset in body frame
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_S_OFF_X, 0.0f);
+
+/**
+ * Sensor Y position offset in body frame
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 2
+ *
+ * @group Landing target Estimator
+ */
+PARAM_DEFINE_FLOAT(LTEST_S_OFF_Y, 0.0f);