diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index bc0249a3ca..37ed5c9516 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/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 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 q_att(&_vehicleAttitude.q[0]); _R_att = matrix::Dcm(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(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); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 55a2d47cb1..91b96ad897 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -58,6 +58,7 @@ #include #include #include +#include #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; diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 62aa6b7e45..24ee9b772f 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/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);