zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

LPE: refactor to use ModuleParams

sbg
Beat Küng 7 years ago
parent
83d55773af
commit
7ee464c264
2 changed files with 72 additions and 114 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 53
      src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
  2. 133
      src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

53
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
Unescape View File

@ -21,6 +21,7 @@ static const char *msg_label = "[lpe] "; // rate of land detector correction @@ -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() : @@ -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() : @@ -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() @@ -323,7 +275,8 @@ void BlockLocalPositionEstimator::update()
// update parameters
if (paramsUpdated) {
updateParams();
SuperBlock::updateParams();
ModuleParams::updateParams();
updateSSParams();
}

133
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
Unescape View File

@ -1,6 +1,7 @@ @@ -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, @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save