fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-16 07:41:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

LTE: use new param sub style

Browse Source
This commit is contained in:
bresch
2022-10-06 16:56:52 +02:00
parent 30150f723a
commit ecdc11edf6
2 changed files with 36 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+19 -46
View File
@@ -50,19 +50,9 @@
namespace landing_target_estimator namespace landing_target_estimator
{ {
LandingTargetEstimator::LandingTargetEstimator() LandingTargetEstimator::LandingTargetEstimator() :
ModuleParams(nullptr)
{ {
_paramHandle.acc_unc = param_find("LTEST_ACC_UNC");
_paramHandle.meas_unc = param_find("LTEST_MEAS_UNC");
_paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN");
_paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN");
_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); _check_params(true);
} }
@@ -93,8 +83,8 @@ void LandingTargetEstimator::update()
a.zero(); a.zero();
} }
_kalman_filter_x.predict(dt, -a(0), _params.acc_unc); _kalman_filter_x.predict(dt, -a(0), _param_ltest_acc_unc.get());
_kalman_filter_y.predict(dt, -a(1), _params.acc_unc); _kalman_filter_y.predict(dt, -a(1), _param_ltest_acc_unc.get());
_last_predict = hrt_absolute_time(); _last_predict = hrt_absolute_time();
} }
@@ -113,8 +103,10 @@ void LandingTargetEstimator::update()
float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init); PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init); _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _param_ltest_pos_unc_in.get(),
_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init); _param_ltest_vel_unc_in.get());
_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _param_ltest_pos_unc_in.get(),
_param_ltest_vel_unc_in.get());
_estimator_initialized = true; _estimator_initialized = true;
_last_update = hrt_absolute_time(); _last_update = hrt_absolute_time();
@@ -122,7 +114,7 @@ void LandingTargetEstimator::update()
} else { } else {
// update // update
const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z; const float measurement_uncertainty = _param_ltest_meas_unc.get() * _dist_z * _dist_z;
bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty); 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); bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
@@ -148,7 +140,7 @@ void LandingTargetEstimator::update()
_kalman_filter_y.getState(y, yvel); _kalman_filter_y.getState(y, yvel);
_kalman_filter_y.getCovariance(covy, covy_v); _kalman_filter_y.getCovariance(covy, covy_v);
_target_pose.is_static = (_params.mode == TargetMode::Stationary); _target_pose.is_static = ((TargetMode)_param_ltest_mode.get() == TargetMode::Stationary);
_target_pose.rel_pos_valid = true; _target_pose.rel_pos_valid = true;
_target_pose.rel_vel_valid = true; _target_pose.rel_vel_valid = true;
@@ -200,7 +192,7 @@ void LandingTargetEstimator::_check_params(const bool force)
parameter_update_s pupdate; parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate); _parameter_update_sub.copy(&pupdate);
_update_params(); updateParams();
} }
} }
@@ -224,12 +216,12 @@ void LandingTargetEstimator::_update_topics()
} }
matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward sensor_ray(0) = _irlockReport.pos_x * _param_ltest_scale_x.get(); // forward
sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right sensor_ray(1) = _irlockReport.pos_y * _param_ltest_scale_y.get(); // right
sensor_ray(2) = 1.0f; sensor_ray(2) = 1.0f;
// rotate unit ray according to sensor orientation // rotate unit ray according to sensor orientation
_S_att = get_rot_matrix(_params.sensor_yaw); _S_att = get_rot_matrix(static_cast<enum Rotation>(_param_ltest_sens_rot.get()));
sensor_ray = _S_att * sensor_ray; sensor_ray = _S_att * sensor_ray;
// rotate the unit ray into the navigation frame // rotate the unit ray into the navigation frame
@@ -242,7 +234,7 @@ void LandingTargetEstimator::_update_topics()
return; return;
} }
_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z; _dist_z = _vehicleLocalPosition.dist_bottom - _param_ltest_sens_pos_z.get();
// scale the ray s.t. the z component has length of _uncertainty_scale // scale the ray s.t. the z component has length of _uncertainty_scale
_target_position_report.timestamp = _irlockReport.timestamp; _target_position_report.timestamp = _irlockReport.timestamp;
@@ -251,8 +243,8 @@ void LandingTargetEstimator::_update_topics()
_target_position_report.rel_pos_z = _dist_z; _target_position_report.rel_pos_z = _dist_z;
// Adjust relative position according to sensor offset // Adjust relative position according to sensor offset
_target_position_report.rel_pos_x += _params.offset_x; _target_position_report.rel_pos_x += _param_ltest_sens_pos_x.get();
_target_position_report.rel_pos_y += _params.offset_y; _target_position_report.rel_pos_y += _param_ltest_sens_pos_y.get();
_new_sensorReport = true; _new_sensorReport = true;
@@ -282,28 +274,9 @@ void LandingTargetEstimator::_update_topics()
} }
} }
void LandingTargetEstimator::_update_params() void LandingTargetEstimator::updateParams()
{ {
param_get(_paramHandle.acc_unc, &_params.acc_unc); ModuleParams::updateParams();
param_get(_paramHandle.meas_unc, &_params.meas_unc);
param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init);
param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init);
int32_t mode = 0;
param_get(_paramHandle.mode, &mode);
_params.mode = (TargetMode)mode;
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);
} }
} // namespace landing_target_estimator } // namespace landing_target_estimator
src/modules/landing_target_estimator/LandingTargetEstimator.h
+17 -33
View File
@@ -42,6 +42,7 @@
#pragma once #pragma once
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/workqueue.h> #include <px4_platform_common/workqueue.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <parameters/param.h> #include <parameters/param.h>
@@ -69,7 +70,7 @@ using namespace time_literals;
namespace landing_target_estimator namespace landing_target_estimator
{ {
class LandingTargetEstimator class LandingTargetEstimator: public ModuleParams
{ {
public: public:
@@ -91,7 +92,7 @@ protected:
/* /*
* Update parameters. * Update parameters.
*/ */
void _update_params(); void updateParams() override;
/* timeout after which filter is reset if target not seen */ /* timeout after which filter is reset if target not seen */
static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000; static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000;
@@ -111,37 +112,6 @@ private:
Stationary Stationary
}; };
/**
* Handles for parameters
**/
struct {
param_t acc_unc;
param_t meas_unc;
param_t pos_unc_init;
param_t vel_unc_init;
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 {
float acc_unc;
float meas_unc;
float pos_unc_init;
float vel_unc_init;
TargetMode mode;
float scale_x;
float scale_y;
float offset_x;
float offset_y;
float offset_z;
enum Rotation sensor_yaw;
} _params;
struct { struct {
hrt_abstime timestamp; hrt_abstime timestamp;
float rel_pos_x; float rel_pos_x;
@@ -184,5 +154,19 @@ private:
void _check_params(const bool force); void _check_params(const bool force);
void _update_state(); void _update_state();
DEFINE_PARAMETERS(
(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_ltest_acc_unc,
(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_ltest_meas_unc,
(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_ltest_pos_unc_in,
(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_ltest_vel_unc_in,
(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
(ParamFloat<px4::params::LTEST_SCALE_X>) _param_ltest_scale_x,
(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_ltest_scale_y,
(ParamInt<px4::params::LTEST_SENS_ROT>) _param_ltest_sens_rot,
(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_ltest_sens_pos_x,
(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_ltest_sens_pos_y,
(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_ltest_sens_pos_z
)
}; };
} // namespace landing_target_estimator } // namespace landing_target_estimator