fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 14:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE: refactor to use ModuleParams

Browse Source
This commit is contained in:
Beat Küng
2018-03-13 17:35:15 +01:00
parent 83d55773af
commit 7ee464c264
2 changed files with 62 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+59 -54
View File
@@ -1,6 +1,7 @@
#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;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart, /**< example parameter */
// lidar parameters
BlockParamFloat _lidar_z_stddev;
BlockParamFloat _lidar_z_offset;
// 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,
// accel parameters
BlockParamFloat _accel_xy_stddev;
BlockParamFloat _accel_z_stddev;
// sonar parameters
(ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
(ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,
// baro parameters
BlockParamFloat _baro_stddev;
// lidar parameters
(ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
(ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,
// 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;
// accel parameters
(ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
(ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,
// vision parameters
BlockParamFloat _vision_xy_stddev;
BlockParamFloat _vision_z_stddev;
BlockParamFloat _vision_delay;
// baro parameters
(ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,
// mocap parameters
BlockParamFloat _mocap_p_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,
// 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;
// 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,
// land parameters
BlockParamFloat _land_z_stddev;
BlockParamFloat _land_vxy_stddev;
// mocap parameters
(ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_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;
// 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;