fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 16:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Parameter update - Rename variables in modules/fw_pos_control

Browse Source 
using parameter_update.py script
This commit is contained in:
bresch
2019-03-25 13:14:21 +01:00
committed by Matthias Grob
parent ecacf4de46
commit f08c00f324
11 changed files with 118 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+36 -36
View File
@@ -170,14 +170,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0,
(_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_fusion.get() & FUSE_BARO) != 0);
(_param_lpe_fusion.get() & FUSE_GPS) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW) != 0,
(_param_lpe_fusion.get() & FUSE_VIS_POS) != 0,
(_param_lpe_fusion.get() & FUSE_LAND_TARGET) != 0,
(_param_lpe_fusion.get() & FUSE_LAND) != 0,
(_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_param_lpe_fusion.get() & FUSE_BARO) != 0);
}
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
@@ -267,16 +267,16 @@ void BlockLocalPositionEstimator::update()
bool paramsUpdated = _sub_param_update.updated();
_baroUpdated = false;
if ((_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.updated()) {
if (_sub_airdata.get().timestamp != _timeStampLastBaro) {
_baroUpdated = true;
_timeStampLastBaro = _sub_airdata.get().timestamp;
}
}
_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
_flowUpdated = (_param_lpe_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
_gpsUpdated = (_param_lpe_fusion.get() & FUSE_GPS) && _sub_gps.updated();
_visionUpdated = (_param_lpe_fusion.get() & FUSE_VIS_POS) && _sub_visual_odom.updated();
_mocapUpdated = _sub_mocap_odom.updated();
_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
@@ -296,7 +296,7 @@ void BlockLocalPositionEstimator::update()
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get()) {
vxy_stddev_ok = true;
}
@@ -321,7 +321,7 @@ void BlockLocalPositionEstimator::update()
}
// is z valid?
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_Z) {
// if valid and baro has timed out, set to not valid
@@ -336,7 +336,7 @@ void BlockLocalPositionEstimator::update()
}
// is terrain valid?
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _param_lpe_z_pub.get();
if (_estimatorInitialized & EST_TZ) {
if (!tz_stddev_ok) {
@@ -353,16 +353,16 @@ void BlockLocalPositionEstimator::update()
checkTimeouts();
// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
map_projection_init(&_map_ref,
(double)_init_origin_lat.get(),
(double)_init_origin_lon.get());
(double)_param_lpe_lat.get(),
(double)_param_lpe_lon.get());
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
@@ -515,7 +515,7 @@ void BlockLocalPositionEstimator::update()
_pub_innov.get().timestamp = _timeStamp;
_pub_innov.update();
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
publishGlobalPos();
}
}
@@ -548,7 +548,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
bool BlockLocalPositionEstimator::landed()
{
if (!(_fusion.get() & FUSE_LAND)) {
if (!(_param_lpe_fusion.get() & FUSE_LAND)) {
return false;
}
@@ -570,7 +570,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (evh < _vxy_pub_thresh.get()) {
if (evh < _param_lpe_vxy_pub.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
@@ -594,7 +594,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
if (_fusion.get() & FUSE_PUB_AGL_Z) {
if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
@@ -654,7 +654,7 @@ void BlockLocalPositionEstimator::publishOdom()
_pub_odom.get().x = xLP(X_x); // north
_pub_odom.get().y = xLP(X_y); // east
if (_fusion.get() & FUSE_PUB_AGL_Z) {
if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_odom.get().z = -_aglLowPass.getState(); // agl
} else {
@@ -781,7 +781,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (evh < _vxy_pub_thresh.get()) {
if (evh < _param_lpe_vxy_pub.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
@@ -818,10 +818,10 @@ void BlockLocalPositionEstimator::initP()
_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;
_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;
_P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_P(X_vx, X_vx) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
_P(X_vy, X_vy) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// use vxy thresh for vz init as well
_P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();
_P(X_vz, X_vz) = 2 * _param_lpe_vxy_pub.get() * _param_lpe_vxy_pub.get();
// initialize bias uncertainty to small values to keep them stable
_P(X_bx, X_bx) = 1e-6;
_P(X_by, X_by) = 1e-6;
@@ -872,14 +872,14 @@ void BlockLocalPositionEstimator::updateSSParams()
{
// input noise covariance matrix
_R.setZero();
_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
_R(U_ax, U_ax) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
_R(U_ay, U_ay) = _param_lpe_acc_xy.get() * _param_lpe_acc_xy.get();
_R(U_az, U_az) = _param_lpe_acc_z.get() * _param_lpe_acc_z.get();
// process noise power matrix
_Q.setZero();
float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
float pn_p_sq = _param_lpe_pn_p.get() * _param_lpe_pn_p.get();
float pn_v_sq = _param_lpe_pn_v.get() * _param_lpe_pn_v.get();
_Q(X_x, X_x) = pn_p_sq;
_Q(X_y, X_y) = pn_p_sq;
_Q(X_z, X_z) = pn_p_sq;
@@ -890,15 +890,15 @@ void BlockLocalPositionEstimator::updateSSParams()
// technically, the noise is in the body frame,
// but the components are all the same, so
// ignoring for now
float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
float pn_b_sq = _param_lpe_pn_b.get() * _param_lpe_pn_b.get();
_Q(X_bx, X_bx) = pn_b_sq;
_Q(X_by, X_by) = pn_b_sq;
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
float pn_t_noise_density =
_pn_t_noise_density.get() +
(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_param_lpe_pn_t.get() +
(_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}