From f08c00f3246f375aaaf8a80c48012c824c463a88 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 25 Mar 2019 13:14:21 +0100
Subject: [PATCH] Parameter update - Rename variables in modules/fw_pos_control
using parameter_update.py script
---
.../BlockLocalPositionEstimator.cpp | 72 ++++++++---------
.../BlockLocalPositionEstimator.hpp | 78 +++++++++----------
.../local_position_estimator/sensors/baro.cpp | 2 +-
.../local_position_estimator/sensors/flow.cpp | 12 +--
.../local_position_estimator/sensors/gps.cpp | 24 +++---
.../local_position_estimator/sensors/land.cpp | 6 +-
.../sensors/landing_target.cpp | 12 +--
.../sensors/lidar.cpp | 4 +-
.../sensors/mocap.cpp | 10 +--
.../sensors/sonar.cpp | 4 +-
.../sensors/vision.cpp | 12 +--
11 files changed, 118 insertions(+), 118 deletions(-)
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 5bb474468a..de0620b9f0 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -170,14 +170,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
- (_fusion.get() & FUSE_GPS) != 0,
- (_fusion.get() & FUSE_FLOW) != 0,
- (_fusion.get() & FUSE_VIS_POS) != 0,
- (_fusion.get() & FUSE_LAND_TARGET) != 0,
- (_fusion.get() & FUSE_LAND) != 0,
- (_fusion.get() & FUSE_PUB_AGL_Z) != 0,
- (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
- (_fusion.get() & FUSE_BARO) != 0);
+ (_param_lpe_fusion.get() & FUSE_GPS) != 0,
+ (_param_lpe_fusion.get() & FUSE_FLOW) != 0,
+ (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
+ (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
+ (_param_lpe_fusion.get() & FUSE_LAND) != 0,
+ (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
+ (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
+ (_param_lpe_fusion.get() & FUSE_BARO) != 0);
}
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
@@ -267,16 +267,16 @@ void BlockLocalPositionEstimator::update()
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
- if ((_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
+ if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = _sub_airdata.get().timestamp;
}
}
- _flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
- _gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
- _visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
+ _flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
+ _gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.updated();
+ _visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
_mocapUpdated = _sub_mocap_odom.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
@@ -296,7 +296,7 @@ void BlockLocalPositionEstimator::update()
// is xy valid?
bool vxy_stddev_ok = false;
- if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
+ if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
vxy_stddev_ok = true;
}
@@ -321,7 +321,7 @@ void BlockLocalPositionEstimator::update()
}
// is z valid?
- bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
+ bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_Z) {
// if valid and baro has timed out, set to not valid
@@ -336,7 +336,7 @@ void BlockLocalPositionEstimator::update()
}
// is terrain valid?
- bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
+ bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
@@ -353,16 +353,16 @@ void BlockLocalPositionEstimator::update()
checkTimeouts();
// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
- if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
+ if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
map_projection_init(&_map_ref,
- (double)_init_origin_lat.get(),
- (double)_init_origin_lon.get());
+ (double)_param_lpe_lat.get(),
+ (double)_param_lpe_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
- double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
+ double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
@@ -515,7 +515,7 @@ void BlockLocalPositionEstimator::update()
_pub_innov.get().timestamp = _timeStamp;
_pub_innov.update();
- if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
+ if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
publishGlobalPos();
}
}
@@ -548,7 +548,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
bool BlockLocalPositionEstimator::landed()
{
- if (!(_fusion.get() & FUSE_LAND)) {
+ if (!(_param_lpe_fusion.get() & FUSE_LAND)) {
return false;
}
@@ -570,7 +570,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
- if (evh < _vxy_pub_thresh.get()) {
+ if (evh < _param_lpe_vxy_pub.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
@@ -594,7 +594,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
- if (_fusion.get() & FUSE_PUB_AGL_Z) {
+ if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
@@ -654,7 +654,7 @@ void BlockLocalPositionEstimator::publishOdom()
_pub_odom.get().x = xLP(X_x); // north
_pub_odom.get().y = xLP(X_y); // east
- if (_fusion.get() & FUSE_PUB_AGL_Z) {
+ if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_odom.get().z = -_aglLowPass.getState(); // agl
} else {
@@ -781,7 +781,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
- if (evh < _vxy_pub_thresh.get()) {
+ if (evh < _param_lpe_vxy_pub.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
@@ -818,10 +818,10 @@ void BlockLocalPositionEstimator::initP()
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
- _P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
- _P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
+ _P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
+ _P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// use vxy thresh for vz init as well
- _P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
+ _P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// initialize bias uncertainty to small values to keep them stable
_P(X_bx, X_bx) = 1e-6;
_P(X_by, X_by) = 1e-6;
@@ -872,14 +872,14 @@ void BlockLocalPositionEstimator::updateSSParams()
{
// input noise covariance matrix
_R.setZero();
- _R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
- _R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
- _R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
+ _R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
+ _R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
+ _R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
// process noise power matrix
_Q.setZero();
- float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
- float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
+ float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
+ float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
_Q(X_x, X_x) = pn_p_sq;
_Q(X_y, X_y) = pn_p_sq;
_Q(X_z, X_z) = pn_p_sq;
@@ -890,15 +890,15 @@ void BlockLocalPositionEstimator::updateSSParams()
// technically, the noise is in the body frame,
// but the components are all the same, so
// ignoring for now
- float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
+ float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
_Q(X_bx, X_bx) = pn_b_sq;
_Q(X_by, X_by) = pn_b_sq;
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
float pn_t_noise_density =
- _pn_t_noise_density.get() +
- (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+ _param_lpe_pn_t.get() +
+ (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 478e8a570b..7c816533ee 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -277,70 +277,70 @@ private:
DEFINE_PARAMETERS(
- (ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart, /**< example parameter */
+ (ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
// general parameters
- (ParamInt<px4::params::LPE_FUSION>) _fusion,
- (ParamFloat<px4::params::LPE_VXY_PUB>) _vxy_pub_thresh,
- (ParamFloat<px4::params::LPE_Z_PUB>) _z_pub_thresh,
+ (ParamInt<px4::params::LPE_FUSION>) _param_lpe_fusion,
+ (ParamFloat<px4::params::LPE_VXY_PUB>) _param_lpe_vxy_pub,
+ (ParamFloat<px4::params::LPE_Z_PUB>) _param_lpe_z_pub,
// sonar parameters
- (ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
- (ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
+ (ParamFloat<px4::params::LPE_SNR_Z>) _param_lpe_snr_z,
+ (ParamFloat<px4::params::LPE_SNR_OFF_Z>) _param_lpe_snr_off_z,
// lidar parameters
- (ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
- (ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
+ (ParamFloat<px4::params::LPE_LDR_Z>) _param_lpe_ldr_z,
+ (ParamFloat<px4::params::LPE_LDR_OFF_Z>) _param_lpe_ldr_off_z,
// accel parameters
- (ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
- (ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
+ (ParamFloat<px4::params::LPE_ACC_XY>) _param_lpe_acc_xy,
+ (ParamFloat<px4::params::LPE_ACC_Z>) _param_lpe_acc_z,
// baro parameters
- (ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,
+ (ParamFloat<px4::params::LPE_BAR_Z>) _param_lpe_bar_z,
// gps parameters
- (ParamFloat<px4::params::LPE_GPS_DELAY>) _gps_delay,
- (ParamFloat<px4::params::LPE_GPS_XY>) _gps_xy_stddev,
- (ParamFloat<px4::params::LPE_GPS_Z>) _gps_z_stddev,
- (ParamFloat<px4::params::LPE_GPS_VXY>) _gps_vxy_stddev,
- (ParamFloat<px4::params::LPE_GPS_VZ>) _gps_vz_stddev,
- (ParamFloat<px4::params::LPE_EPH_MAX>) _gps_eph_max,
- (ParamFloat<px4::params::LPE_EPV_MAX>) _gps_epv_max,
+ (ParamFloat<px4::params::LPE_GPS_DELAY>) _param_lpe_gps_delay,
+ (ParamFloat<px4::params::LPE_GPS_XY>) _param_lpe_gps_xy,
+ (ParamFloat<px4::params::LPE_GPS_Z>) _param_lpe_gps_z,
+ (ParamFloat<px4::params::LPE_GPS_VXY>) _param_lpe_gps_vxy,
+ (ParamFloat<px4::params::LPE_GPS_VZ>) _param_lpe_gps_vz,
+ (ParamFloat<px4::params::LPE_EPH_MAX>) _param_lpe_eph_max,
+ (ParamFloat<px4::params::LPE_EPV_MAX>) _param_lpe_epv_max,
// vision parameters
- (ParamFloat<px4::params::LPE_VIS_XY>) _vision_xy_stddev,
- (ParamFloat<px4::params::LPE_VIS_Z>) _vision_z_stddev,
- (ParamFloat<px4::params::LPE_VIS_DELAY>) _vision_delay,
+ (ParamFloat<px4::params::LPE_VIS_XY>) _param_lpe_vis_xy,
+ (ParamFloat<px4::params::LPE_VIS_Z>) _param_lpe_vis_z,
+ (ParamFloat<px4::params::LPE_VIS_DELAY>) _param_lpe_vis_delay,
// mocap parameters
- (ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_stddev,
+ (ParamFloat<px4::params::LPE_VIC_P>) _param_lpe_vic_p,
// flow parameters
- (ParamFloat<px4::params::LPE_FLW_OFF_Z>) _flow_z_offset,
- (ParamFloat<px4::params::LPE_FLW_SCALE>) _flow_scale,
- (ParamInt<px4::params::LPE_FLW_QMIN>) _flow_min_q,
- (ParamFloat<px4::params::LPE_FLW_R>) _flow_r,
- (ParamFloat<px4::params::LPE_FLW_RR>) _flow_rr,
+ (ParamFloat<px4::params::LPE_FLW_OFF_Z>) _param_lpe_flw_off_z,
+ (ParamFloat<px4::params::LPE_FLW_SCALE>) _param_lpe_flw_scale,
+ (ParamInt<px4::params::LPE_FLW_QMIN>) _param_lpe_flw_qmin,
+ (ParamFloat<px4::params::LPE_FLW_R>) _param_lpe_flw_r,
+ (ParamFloat<px4::params::LPE_FLW_RR>) _param_lpe_flw_rr,
// land parameters
- (ParamFloat<px4::params::LPE_LAND_Z>) _land_z_stddev,
- (ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,
+ (ParamFloat<px4::params::LPE_LAND_Z>) _param_lpe_land_z,
+ (ParamFloat<px4::params::LPE_LAND_VXY>) _param_lpe_land_vxy,
// process noise
- (ParamFloat<px4::params::LPE_PN_P>) _pn_p_noise_density,
- (ParamFloat<px4::params::LPE_PN_V>) _pn_v_noise_density,
- (ParamFloat<px4::params::LPE_PN_B>) _pn_b_noise_density,
- (ParamFloat<px4::params::LPE_PN_T>) _pn_t_noise_density,
- (ParamFloat<px4::params::LPE_T_MAX_GRADE>) _t_max_grade,
+ (ParamFloat<px4::params::LPE_PN_P>) _param_lpe_pn_p,
+ (ParamFloat<px4::params::LPE_PN_V>) _param_lpe_pn_v,
+ (ParamFloat<px4::params::LPE_PN_B>) _param_lpe_pn_b,
+ (ParamFloat<px4::params::LPE_PN_T>) _param_lpe_pn_t,
+ (ParamFloat<px4::params::LPE_T_MAX_GRADE>) _param_lpe_t_max_grade,
- (ParamFloat<px4::params::LPE_LT_COV>) _target_min_cov,
- (ParamInt<px4::params::LTEST_MODE>) _target_mode,
+ (ParamFloat<px4::params::LPE_LT_COV>) _param_lpe_lt_cov,
+ (ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
// init origin
- (ParamInt<px4::params::LPE_FAKE_ORIGIN>) _fake_origin,
- (ParamFloat<px4::params::LPE_LAT>) _init_origin_lat,
- (ParamFloat<px4::params::LPE_LON>) _init_origin_lon
+ (ParamInt<px4::params::LPE_FAKE_ORIGIN>) _param_lpe_fake_origin,
+ (ParamFloat<px4::params::LPE_LAT>) _param_lpe_lat,
+ (ParamFloat<px4::params::LPE_LON>) _param_lpe_lon
)
// target mode paramters from landing_target_estimator module
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 6dfc9d9948..805f869092 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -64,7 +64,7 @@ void BlockLocalPositionEstimator::baroCorrect()
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();
- R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
+ R(0, 0) = _param_lpe_bar_z.get() * _param_lpe_bar_z.get();
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 2475e22f07..bde403358e 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -51,7 +51,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
// check quality
float qual = _sub_flow.get().quality;
- if (qual < _flow_min_q.get()) {
+ if (qual < _param_lpe_flw_qmin.get()) {
return -1;
}
@@ -64,8 +64,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
// optical flow in x, y axis
// TODO consider making flow scale a states of the kalman filter
- float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
- float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
+ float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get();
+ float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get();
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
@@ -76,7 +76,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float gyro_x_rad = 0;
float gyro_y_rad = 0;
- if (_fusion.get() & FUSE_FLOW_GYRO_COMP) {
+ if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) {
gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
gyro_y_rad = _flow_gyro_y_high_pass.update(
@@ -163,8 +163,8 @@ void BlockLocalPositionEstimator::flowCorrect()
float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
- _flow_r.get() * _flow_r.get() * rot_sq +
- _flow_rr.get() * _flow_rr.get() * rotrate_sq;
+ _param_lpe_flw_r.get() * _param_lpe_flw_r.get() * rot_sq +
+ _param_lpe_flw_rr.get() * _param_lpe_flw_rr.get() * rotrate_sq;
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
// residual
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 12eb23d4b8..95ce31c0d7 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -19,8 +19,8 @@ void BlockLocalPositionEstimator::gpsInit()
if (
nSat < 6 ||
- eph > _gps_eph_max.get() ||
- epv > _gps_epv_max.get() ||
+ eph > _param_lpe_eph_max.get() ||
+ epv > _param_lpe_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
@@ -57,7 +57,7 @@ void BlockLocalPositionEstimator::gpsInit()
// find lat, lon of current origin by subtracting x and y
// if not using vision position since vision will
// have it's own origin, not necessarily where vehicle starts
- if (!_map_ref.init_done && !(_fusion.get() & FUSE_VIS_POS)) {
+ if (!_map_ref.init_done && !(_param_lpe_fusion.get() & FUSE_VIS_POS)) {
double gpsLatOrigin = 0;
double gpsLonOrigin = 0;
// reproject at current coordinates
@@ -144,27 +144,27 @@ void BlockLocalPositionEstimator::gpsCorrect()
R.setZero();
// default to parameter, use gps cov if provided
- float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get();
- float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
- float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
- float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
+ float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
+ float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
+ float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
+ float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
// if field is not below minimum, set it to the value provided
- if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
+ if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
- if (_sub_gps.get().epv > _gps_z_stddev.get()) {
+ if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
- if (gps_s_stddev > _gps_vxy_stddev.get()) {
+ if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
- if (gps_s_stddev > _gps_vz_stddev.get()) {
+ if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
@@ -178,7 +178,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
// get delayed x
uint8_t i_hist = 0;
- if (getDelayPeriods(_gps_delay.get(), &i_hist) < 0) { return; }
+ if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp
index c62ca518f3..8e67311192 100644
--- a/src/modules/local_position_estimator/sensors/land.cpp
+++ b/src/modules/local_position_estimator/sensors/land.cpp
@@ -54,9 +54,9 @@ void BlockLocalPositionEstimator::landCorrect()
// use parameter covariance
SquareMatrix<float, n_y_land> R;
R.setZero();
- R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
- R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
- R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
+ R(Y_land_vx, Y_land_vx) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get();
+ R(Y_land_vy, Y_land_vy) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get();
+ R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get();
// residual
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp
index a2fbacfec0..0c389ed686 100644
--- a/src/modules/local_position_estimator/sensors/landing_target.cpp
+++ b/src/modules/local_position_estimator/sensors/landing_target.cpp
@@ -8,7 +8,7 @@ static const uint64_t TARGET_TIMEOUT = 2000000; // [us]
void BlockLocalPositionEstimator::landingTargetInit()
{
- if (_target_mode.get() == Target_Moving) {
+ if (_param_ltest_mode.get() == Target_Moving) {
// target is in moving mode, do not initialize
return;
}
@@ -24,7 +24,7 @@ void BlockLocalPositionEstimator::landingTargetInit()
int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target> &y)
{
- if (_target_mode.get() == Target_Stationary) {
+ if (_param_ltest_mode.get() == Target_Stationary) {
if (_sub_landing_target_pose.get().rel_vel_valid) {
y(0) = _sub_landing_target_pose.get().vx_rel;
y(1) = _sub_landing_target_pose.get().vy_rel;
@@ -43,7 +43,7 @@ int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target>
void BlockLocalPositionEstimator::landingTargetCorrect()
{
- if (_target_mode.get() == Target_Moving) {
+ if (_param_ltest_mode.get() == Target_Moving) {
// nothing to do in this mode
return;
}
@@ -58,9 +58,9 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
float cov_vy = _sub_landing_target_pose.get().cov_vy_rel;
// use sensor value only if reasoanble
- if (cov_vx < _target_min_cov.get() || cov_vy < _target_min_cov.get()) {
- cov_vx = _target_min_cov.get();
- cov_vy = _target_min_cov.get();
+ if (cov_vx < _param_lpe_lt_cov.get() || cov_vy < _param_lpe_lt_cov.get()) {
+ cov_vx = _param_lpe_lt_cov.get();
+ cov_vy = _param_lpe_lt_cov.get();
}
// target measurement matrix and noise matrix
diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp
index 0f1a3dafc6..126c943601 100644
--- a/src/modules/local_position_estimator/sensors/lidar.cpp
+++ b/src/modules/local_position_estimator/sensors/lidar.cpp
@@ -53,7 +53,7 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
_time_last_lidar = _timeStamp;
y.setZero();
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
- y(0) = (d + _lidar_z_offset.get()) *
+ y(0) = (d + _param_lpe_ldr_off_z.get()) *
cosf(euler.phi()) *
cosf(euler.theta());
return OK;
@@ -80,7 +80,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
float cov = _sub_lidar->get().variance;
if (cov < 1.0e-3f) {
- R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get();
+ R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
} else {
R(0, 0) = cov;
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index 2ad5983bfb..cdbef158a8 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -122,20 +122,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
R.setZero();
// use std dev from mocap data if available
- if (_mocap_eph > _mocap_p_stddev.get()) {
+ if (_mocap_eph > _param_lpe_vic_p.get()) {
R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph;
R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph;
} else {
- R(Y_mocap_x, Y_mocap_x) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
- R(Y_mocap_y, Y_mocap_y) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
+ R(Y_mocap_x, Y_mocap_x) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
+ R(Y_mocap_y, Y_mocap_y) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
- if (_mocap_epv > _mocap_p_stddev.get()) {
+ if (_mocap_epv > _param_lpe_vic_p.get()) {
R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv;
} else {
- R(Y_mocap_z, Y_mocap_z) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
+ R(Y_mocap_z, Y_mocap_z) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
// residual
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index 6071853bad..ecc8b063c1 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -67,7 +67,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
_time_last_sonar = _timeStamp;
y.setZero();
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
- y(0) = (d + _sonar_z_offset.get()) *
+ y(0) = (d + _param_lpe_snr_off_z.get()) *
cosf(euler.phi()) *
cosf(euler.theta());
return OK;
@@ -90,7 +90,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
if (cov < 1.0e-3f) {
// use sensor value if reasoanble
- cov = _sonar_z_stddev.get() * _sonar_z_stddev.get();
+ cov = _param_lpe_snr_z.get() * _param_lpe_snr_z.get();
}
// sonar measurement matrix and noise matrix
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index 71dbd528d7..14604e99e2 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -127,20 +127,20 @@ void BlockLocalPositionEstimator::visionCorrect()
R.setZero();
// use std dev from vision data if available
- if (_vision_eph > _vision_xy_stddev.get()) {
+ if (_vision_eph > _param_lpe_vis_xy.get()) {
R(Y_vision_x, Y_vision_x) = _vision_eph * _vision_eph;
R(Y_vision_y, Y_vision_y) = _vision_eph * _vision_eph;
} else {
- R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
- R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
+ R(Y_vision_x, Y_vision_x) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get();
+ R(Y_vision_y, Y_vision_y) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get();
}
- if (_vision_epv > _vision_z_stddev.get()) {
+ if (_vision_epv > _param_lpe_vis_z.get()) {
R(Y_vision_z, Y_vision_z) = _vision_epv * _vision_epv;
} else {
- R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
+ R(Y_vision_z, Y_vision_z) = _param_lpe_vis_z.get() * _param_lpe_vis_z.get();
}
// vision delayed x
@@ -151,7 +151,7 @@ void BlockLocalPositionEstimator::visionCorrect()
if (vision_delay < 0.0f) { vision_delay = 0.0f; }
// use auto-calculated delay from measurement if parameter is set to zero
- if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; }
+ if (getDelayPeriods(_param_lpe_vis_delay.get() > 0.0f ? _param_lpe_vis_delay.get() : vision_delay, &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
--
GitLab