diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 94252d21e4..6758bd6d2b 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/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(); } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index eb95701a78..0a3115645c 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -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) _sys_autostart, /**< example parameter */ + + // general parameters + (ParamInt) _fusion, + (ParamFloat) _vxy_pub_thresh, + (ParamFloat) _z_pub_thresh, + + // sonar parameters + (ParamFloat) _sonar_z_stddev, + (ParamFloat) _sonar_z_offset, + + // lidar parameters + (ParamFloat) _lidar_z_stddev, + (ParamFloat) _lidar_z_offset, + + // accel parameters + (ParamFloat) _accel_xy_stddev, + (ParamFloat) _accel_z_stddev, + + // baro parameters + (ParamFloat) _baro_stddev, + + // gps parameters + (ParamFloat) _gps_delay, + (ParamFloat) _gps_xy_stddev, + (ParamFloat) _gps_z_stddev, + (ParamFloat) _gps_vxy_stddev, + (ParamFloat) _gps_vz_stddev, + (ParamFloat) _gps_eph_max, + (ParamFloat) _gps_epv_max, + + // vision parameters + (ParamFloat) _vision_xy_stddev, + (ParamFloat) _vision_z_stddev, + (ParamFloat) _vision_delay, + + // mocap parameters + (ParamFloat) _mocap_p_stddev, + + // flow parameters + (ParamFloat) _flow_z_offset, + (ParamFloat) _flow_scale, + (ParamInt) _flow_min_q, + (ParamFloat) _flow_r, + (ParamFloat) _flow_rr, + + // land parameters + (ParamFloat) _land_z_stddev, + (ParamFloat) _land_vxy_stddev, + + // process noise + (ParamFloat) _pn_p_noise_density, + (ParamFloat) _pn_v_noise_density, + (ParamFloat) _pn_b_noise_density, + (ParamFloat) _pn_t_noise_density, + (ParamFloat) _t_max_grade, + + (ParamFloat) _target_min_cov, + (ParamInt) _target_mode, + + // init origin + (ParamInt) _fake_origin, + (ParamFloat) _init_origin_lat, + (ParamFloat) _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;