Skip to content

Commit

Permalink
landing_target_estimator: Add offset parameters for a landing target
Browse files Browse the repository at this point in the history
  • Loading branch information
dvornikov-aa committed Dec 2, 2019
1 parent 1148d27 commit 8b9e52b
Show file tree
Hide file tree
Showing 3 changed files with 38 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ 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");

// Initialize uORB topics.
_initialize_topics();
Expand Down Expand Up @@ -153,8 +155,8 @@ void LandingTargetEstimator::update()
float dist = _vehicleLocalPosition.dist_bottom;

// 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;
_rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist + _params.offset_x;
_rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist + _params.offset_y;

if (!_estimator_initialized) {
PX4_INFO("Init");
Expand Down Expand Up @@ -318,6 +320,8 @@ void LandingTargetEstimator::_update_params()
_params.mode = (TargetMode)mode;
param_get(_paramHandle.scale_x, &_params.scale_x);
param_get(_paramHandle.scale_y, &_params.scale_y);
param_get(_paramHandle.offset_x, &_params.offset_x);
param_get(_paramHandle.offset_y, &_params.offset_y);
}


Expand Down
4 changes: 4 additions & 0 deletions src/modules/landing_target_estimator/LandingTargetEstimator.h
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,8 @@ class LandingTargetEstimator
param_t mode;
param_t scale_x;
param_t scale_y;
param_t offset_x;
param_t offset_y;
} _paramHandle;

struct {
Expand All @@ -136,6 +138,8 @@ class LandingTargetEstimator
TargetMode mode;
float scale_x;
float scale_y;
float offset_x;
float offset_y;
} _params;

int _vehicleLocalPositionSub;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,34 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f);
*/
PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f);

/**
* Offset for sensor measurements in sensor x axis
*
* This offset is added to landing target x measurements before they are used
*
* @unit m
* @min -5.0
* @max 5.0
* @decimal 2
*
* @group Landing target Estimator
*/
PARAM_DEFINE_FLOAT(LTEST_OFFSET_X, 0.0f);

/**
* Offset for sensor measurements in sensor y axis
*
* This offset is added to landing target y measurements before they are used
*
* @unit m
* @min -5.0
* @max 5.0
* @decimal 2
*
* @group Landing target Estimator
*/
PARAM_DEFINE_FLOAT(LTEST_OFFSET_Y, 0.0f);

/**
* Landing target estimator
*
Expand Down

0 comments on commit 8b9e52b

Please sign in to comment.