Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

LPE: refactor to use ModuleParams

Browse Source
This commit is contained in:
Beat Küng
2018-03-03 19:56:02 +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.cpp
+3 -50
View File
@@ -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
+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;