Parameter update - Rename variables in modules/fw_pos_control

using parameter_update.py script

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,
+	       (_param_lpe_fusion.get() & FUSE_GPS) != 0,
-	       (_fusion.get() & FUSE_FLOW) != 0,
+	       (_param_lpe_fusion.get() & FUSE_FLOW) != 0,
-	       (_fusion.get() & FUSE_VIS_POS) != 0,
+	       (_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
-	       (_fusion.get() & FUSE_LAND_TARGET) != 0,
+	       (_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
-	       (_fusion.get() & FUSE_LAND) != 0,
+	       (_param_lpe_fusion.get() & FUSE_LAND) != 0,
-	       (_fusion.get() & FUSE_PUB_AGL_Z) != 0,
+	       (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
-	       (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
+	       (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
-	       (_fusion.get() & FUSE_BARO) != 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();
+	_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
-	_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
+	_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.updated();
-	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.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)_param_lpe_lat.get(),
-				    (double)_init_origin_lon.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_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
-	_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.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_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
-	_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
-	_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.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_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
-	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.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() +
+		_param_lpe_pn_t.get() +
-		(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+		(_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;
 }
 

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,
+		(ParamInt<px4::params::LPE_FUSION>) _param_lpe_fusion,
-		(ParamFloat<px4::params::LPE_VXY_PUB>) _vxy_pub_thresh,
+		(ParamFloat<px4::params::LPE_VXY_PUB>) _param_lpe_vxy_pub,
-		(ParamFloat<px4::params::LPE_Z_PUB>) _z_pub_thresh,
+		(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_Z>) _param_lpe_snr_z,
-		(ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
+		(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_Z>) _param_lpe_ldr_z,
-		(ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
+		(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_XY>) _param_lpe_acc_xy,
-		(ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
+		(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_DELAY>) _param_lpe_gps_delay,
-		(ParamFloat<px4::params::LPE_GPS_XY>) _gps_xy_stddev,
+		(ParamFloat<px4::params::LPE_GPS_XY>) _param_lpe_gps_xy,
-		(ParamFloat<px4::params::LPE_GPS_Z>) _gps_z_stddev,
+		(ParamFloat<px4::params::LPE_GPS_Z>) _param_lpe_gps_z,
-		(ParamFloat<px4::params::LPE_GPS_VXY>) _gps_vxy_stddev,
+		(ParamFloat<px4::params::LPE_GPS_VXY>) _param_lpe_gps_vxy,
-		(ParamFloat<px4::params::LPE_GPS_VZ>) _gps_vz_stddev,
+		(ParamFloat<px4::params::LPE_GPS_VZ>) _param_lpe_gps_vz,
-		(ParamFloat<px4::params::LPE_EPH_MAX>) _gps_eph_max,
+		(ParamFloat<px4::params::LPE_EPH_MAX>) _param_lpe_eph_max,
-		(ParamFloat<px4::params::LPE_EPV_MAX>) _gps_epv_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_XY>) _param_lpe_vis_xy,
-		(ParamFloat<px4::params::LPE_VIS_Z>) _vision_z_stddev,
+		(ParamFloat<px4::params::LPE_VIS_Z>) _param_lpe_vis_z,
-		(ParamFloat<px4::params::LPE_VIS_DELAY>) _vision_delay,
+		(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_OFF_Z>) _param_lpe_flw_off_z,
-		(ParamFloat<px4::params::LPE_FLW_SCALE>) _flow_scale,
+		(ParamFloat<px4::params::LPE_FLW_SCALE>) _param_lpe_flw_scale,
-		(ParamInt<px4::params::LPE_FLW_QMIN>) _flow_min_q,
+		(ParamInt<px4::params::LPE_FLW_QMIN>) _param_lpe_flw_qmin,
-		(ParamFloat<px4::params::LPE_FLW_R>) _flow_r,
+		(ParamFloat<px4::params::LPE_FLW_R>) _param_lpe_flw_r,
-		(ParamFloat<px4::params::LPE_FLW_RR>) _flow_rr,
+		(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_Z>) _param_lpe_land_z,
-		(ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,
+		(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_P>) _param_lpe_pn_p,
-		(ParamFloat<px4::params::LPE_PN_V>) _pn_v_noise_density,
+		(ParamFloat<px4::params::LPE_PN_V>) _param_lpe_pn_v,
-		(ParamFloat<px4::params::LPE_PN_B>) _pn_b_noise_density,
+		(ParamFloat<px4::params::LPE_PN_B>) _param_lpe_pn_b,
-		(ParamFloat<px4::params::LPE_PN_T>) _pn_t_noise_density,
+		(ParamFloat<px4::params::LPE_PN_T>) _param_lpe_pn_t,
-		(ParamFloat<px4::params::LPE_T_MAX_GRADE>) _t_max_grade,
+		(ParamFloat<px4::params::LPE_T_MAX_GRADE>) _param_lpe_t_max_grade,
 
-		(ParamFloat<px4::params::LPE_LT_COV>) _target_min_cov,
+		(ParamFloat<px4::params::LPE_LT_COV>) _param_lpe_lt_cov,
-		(ParamInt<px4::params::LTEST_MODE>) _target_mode,
+		(ParamInt<px4::params::LTEST_MODE>) _param_ltest_mode,
 
 		// init origin
-		(ParamInt<px4::params::LPE_FAKE_ORIGIN>) _fake_origin,
+		(ParamInt<px4::params::LPE_FAKE_ORIGIN>) _param_lpe_fake_origin,
-		(ParamFloat<px4::params::LPE_LAT>) _init_origin_lat,
+		(ParamFloat<px4::params::LPE_LAT>) _param_lpe_lat,
-		(ParamFloat<px4::params::LPE_LON>) _init_origin_lon
+		(ParamFloat<px4::params::LPE_LON>) _param_lpe_lon
 	)
 
 	// target mode paramters from landing_target_estimator module

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 =

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_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 * _flow_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 +
+				  _param_lpe_flw_r.get() * _param_lpe_flw_r.get() * rot_sq +
-				  _flow_rr.get() * _flow_rr.get() * rotrate_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

src/modules/local_position_estimator/sensors/gps.cpp

@@ -19,8 +19,8 @@ void BlockLocalPositionEstimator::gpsInit()
 
 	if (
 		nSat < 6 ||
-		eph > _gps_eph_max.get() ||
+		eph > _param_lpe_eph_max.get() ||
-		epv > _gps_epv_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_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
-	float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
+	float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
-	float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
+	float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
-	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.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);
 

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_vx, Y_land_vx) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get();
-	R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.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) = _land_z_stddev.get() * _land_z_stddev.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);

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()) {
+	if (cov_vx < _param_lpe_lt_cov.get() || cov_vy < _param_lpe_lt_cov.get()) {
-		cov_vx = _target_min_cov.get();
+		cov_vx = _param_lpe_lt_cov.get();
-		cov_vy = _target_min_cov.get();
+		cov_vy = _param_lpe_lt_cov.get();
 	}
 
 	// target measurement matrix and noise matrix

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;

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_x, Y_mocap_x) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
-		R(Y_mocap_y, Y_mocap_y) = _mocap_p_stddev.get() * _mocap_p_stddev.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

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

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_x, Y_vision_x) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get();
-		R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.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);