LPE: refactor to use ModuleParams

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -21,6 +21,7 @@ static const char *msg_label = "[lpe] ";	// rate of land detector correction
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(nullptr, "LPE"),
+	ModuleParams(nullptr),
 	// subscriptions, set rate, add to list
 	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
 	_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
@@ -56,52 +57,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// map projection
 	_map_ref(),
 
-	// block parameters
-	_fusion(this, "FUSION"),
-	_vxy_pub_thresh(this, "VXY_PUB"),
-	_z_pub_thresh(this, "Z_PUB"),
-	_sonar_z_stddev(this, "SNR_Z"),
-	_sonar_z_offset(this, "SNR_OFF_Z"),
-	_lidar_z_stddev(this, "LDR_Z"),
-	_lidar_z_offset(this, "LDR_OFF_Z"),
-	_accel_xy_stddev(this, "ACC_XY"),
-	_accel_z_stddev(this, "ACC_Z"),
-	_baro_stddev(this, "BAR_Z"),
-	_gps_delay(this, "GPS_DELAY"),
-	_gps_xy_stddev(this, "GPS_XY"),
-	_gps_z_stddev(this, "GPS_Z"),
-	_gps_vxy_stddev(this, "GPS_VXY"),
-	_gps_vz_stddev(this, "GPS_VZ"),
-	_gps_eph_max(this, "EPH_MAX"),
-	_gps_epv_max(this, "EPV_MAX"),
-	_vision_xy_stddev(this, "VIS_XY"),
-	_vision_z_stddev(this, "VIS_Z"),
-	_vision_delay(this, "VIS_DELAY"),
-	_mocap_p_stddev(this, "VIC_P"),
-	_flow_z_offset(this, "FLW_OFF_Z"),
-	_flow_scale(this, "FLW_SCALE"),
-	//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
-	//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
-	_flow_min_q(this, "FLW_QMIN"),
-	_flow_r(this, "FLW_R"),
-	_flow_rr(this, "FLW_RR"),
-	_land_z_stddev(this, "LAND_Z"),
-	_land_vxy_stddev(this, "LAND_VXY"),
-	_pn_p_noise_density(this, "PN_P"),
-	_pn_v_noise_density(this, "PN_V"),
-	_pn_b_noise_density(this, "PN_B"),
-	_pn_t_noise_density(this, "PN_T"),
-	_t_max_grade(this, "T_MAX_GRADE"),
-
-	// landing target
-	_target_min_cov(this, "LT_COV"),
-	_target_mode(this, "LTEST_MODE", false),
-
-	// init origin
-	_fake_origin(this, "FAKE_ORIGIN"),
-	_init_origin_lat(this, "LAT"),
-	_init_origin_lon(this, "LON"),
-
 	// flow gyro
 	_flow_gyro_x_high_pass(this, "FGYRO_HP"),
 	_flow_gyro_y_high_pass(this, "FGYRO_HP"),
@@ -191,9 +146,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// map
 	_map_ref.init_done = false;
 
-	// intialize parameter dependent matrices
-	updateParams();
-
 	// print fusion settings to console
 	printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
 	       "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
@@ -323,7 +275,8 @@ void BlockLocalPositionEstimator::update()
 
 	// update parameters
 	if (paramsUpdated) {
-		updateParams();
+		SuperBlock::updateParams();
+		ModuleParams::updateParams();
 		updateSSParams();
 	}
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

 #pragma once
 
 #include <px4_posix.h>
+#include <px4_module_params.h>
 #include <controllib/blocks.hpp>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
@@ -52,7 +53,7 @@ static const float BETA_TABLE[7] = {0,
 				    19.6465647819,
 				   };
 
-class BlockLocalPositionEstimator : public control::SuperBlock
+class BlockLocalPositionEstimator : public control::SuperBlock, public ModuleParams
 {
 // dynamics:
 //
@@ -269,75 +270,79 @@ private:
 	// map projection
 	struct map_projection_reference_s _map_ref;
 
-	// general parameters
-	BlockParamInt _fusion;
-	BlockParamFloat  _vxy_pub_thresh;
-	BlockParamFloat  _z_pub_thresh;
-
-	// sonar parameters
-	BlockParamFloat  _sonar_z_stddev;
-	BlockParamFloat  _sonar_z_offset;
-
-	// lidar parameters
-	BlockParamFloat  _lidar_z_stddev;
-	BlockParamFloat  _lidar_z_offset;
-
-	// accel parameters
-	BlockParamFloat  _accel_xy_stddev;
-	BlockParamFloat  _accel_z_stddev;
-
-	// baro parameters
-	BlockParamFloat  _baro_stddev;
-
-	// gps parameters
-	BlockParamFloat  _gps_delay;
-	BlockParamFloat  _gps_xy_stddev;
-	BlockParamFloat  _gps_z_stddev;
-	BlockParamFloat  _gps_vxy_stddev;
-	BlockParamFloat  _gps_vz_stddev;
-	BlockParamFloat  _gps_eph_max;
-	BlockParamFloat  _gps_epv_max;
-
-	// vision parameters
-	BlockParamFloat  _vision_xy_stddev;
-	BlockParamFloat  _vision_z_stddev;
-	BlockParamFloat  _vision_delay;
-
-	// mocap parameters
-	BlockParamFloat  _mocap_p_stddev;
-
-	// flow parameters
-	BlockParamFloat  _flow_z_offset;
-	BlockParamFloat  _flow_scale;
-	//BlockParamFloat  _flow_board_x_offs;
-	//BlockParamFloat  _flow_board_y_offs;
-	BlockParamInt    _flow_min_q;
-	BlockParamFloat  _flow_r;
-	BlockParamFloat  _flow_rr;
-
-	// land parameters
-	BlockParamFloat  _land_z_stddev;
-	BlockParamFloat  _land_vxy_stddev;
-
-	// process noise
-	BlockParamFloat  _pn_p_noise_density;
-	BlockParamFloat  _pn_v_noise_density;
-	BlockParamFloat  _pn_b_noise_density;
-	BlockParamFloat  _pn_t_noise_density;
-	BlockParamFloat  _t_max_grade;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_AUTOSTART>) _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,
+
+		// sonar parameters
+		(ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
+		(ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
+
+		// lidar parameters
+		(ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
+		(ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
+
+		// accel parameters
+		(ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
+		(ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
+
+		// baro parameters
+		(ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,
+
+		// 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,
+
+		// 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,
+
+		// mocap parameters
+		(ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_stddev,
+
+		// 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,
+
+		// land parameters
+		(ParamFloat<px4::params::LPE_LAND_Z>) _land_z_stddev,
+		(ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,
+
+		// 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_LT_COV>) _target_min_cov,
+		(ParamInt<px4::params::LTEST_MODE>) _target_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
+	)
 
 	// target mode paramters from landing_target_estimator module
 	enum TargetMode {
 		Target_Moving = 0,
 		Target_Stationary = 1
 	};
-	BlockParamFloat _target_min_cov;
-	BlockParamInt 	_target_mode;
-
-	// init origin
-	BlockParamInt    _fake_origin;
-	BlockParamFloat  _init_origin_lat;
-	BlockParamFloat  _init_origin_lon;
 
 	// flow gyro filter
 	BlockHighPass _flow_gyro_x_high_pass;