dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Parameter update - Rename variables in modules/fw_pos_control 

using parameter_update.py script
This commit is contained in:
bresch 2019-03-25 13:14:21 +01:00 committed by Matthias Grob
parent ecacf4de46
commit f08c00f324
11 changed files with 118 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -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;
}

78
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -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

2
src/modules/local_position_estimator/sensors/baro.cpp
View File

@ -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 =

12
src/modules/local_position_estimator/sensors/flow.cpp
View File

@ -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

24
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -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);

6
src/modules/local_position_estimator/sensors/land.cpp
View File

@ -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);

12
src/modules/local_position_estimator/sensors/landing_target.cpp
View File

@ -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

4
src/modules/local_position_estimator/sensors/lidar.cpp
View File

@ -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;

10
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -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

4
src/modules/local_position_estimator/sensors/sonar.cpp
View File

@ -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

12
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -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);