fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 07:31:36 +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-04-03 15:38:50 +02: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/sensors/baro.cpp
+1 -1
View File
@@ -64,7 +64,7 @@ void BlockLocalPositionEstimator::baroCorrect()
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();
R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
R(0, 0) = _param_lpe_bar_z.get() * _param_lpe_bar_z.get();
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
src/modules/local_position_estimator/sensors/flow.cpp
+6 -6
View File
@@ -51,7 +51,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
if (qual < _param_lpe_flw_qmin.get()) {
return -1;
}
@@ -64,8 +64,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
// optical flow in x, y axis
// TODO consider making flow scale a states of the kalman filter
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get();
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get();
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
@@ -76,7 +76,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float gyro_x_rad = 0;
float gyro_y_rad = 0;
if (_fusion.get() & FUSE_FLOW_GYRO_COMP) {
if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) {
gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
gyro_y_rad = _flow_gyro_y_high_pass.update(
@@ -163,8 +163,8 @@ void BlockLocalPositionEstimator::flowCorrect()
float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
_flow_r.get() * _flow_r.get() * rot_sq +
_flow_rr.get() * _flow_rr.get() * rotrate_sq;
_param_lpe_flw_r.get() * _param_lpe_flw_r.get() * rot_sq +
_param_lpe_flw_rr.get() * _param_lpe_flw_rr.get() * rotrate_sq;
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
// residual
src/modules/local_position_estimator/sensors/gps.cpp
+12 -12
View File
@@ -19,8 +19,8 @@ void BlockLocalPositionEstimator::gpsInit()
if (
nSat < 6 ||
eph > _gps_eph_max.get() ||
epv > _gps_epv_max.get() ||
eph > _param_lpe_eph_max.get() ||
epv > _param_lpe_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
@@ -57,7 +57,7 @@ void BlockLocalPositionEstimator::gpsInit()
// find lat, lon of current origin by subtracting x and y
// if not using vision position since vision will
// have it's own origin, not necessarily where vehicle starts
if (!_map_ref.init_done && !(_fusion.get() & FUSE_VIS_POS)) {
if (!_map_ref.init_done && !(_param_lpe_fusion.get() & FUSE_VIS_POS)) {
double gpsLatOrigin = 0;
double gpsLonOrigin = 0;
// reproject at current coordinates
@@ -144,27 +144,27 @@ void BlockLocalPositionEstimator::gpsCorrect()
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get();
float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
// if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > _gps_z_stddev.get()) {
if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _gps_vxy_stddev.get()) {
if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _gps_vz_stddev.get()) {
if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
@@ -178,7 +178,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
// get delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_gps_delay.get(), &i_hist) < 0) { return; }
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
src/modules/local_position_estimator/sensors/land.cpp
+3 -3
View File
@@ -54,9 +54,9 @@ void BlockLocalPositionEstimator::landCorrect()
// use parameter covariance
SquareMatrix<float, n_y_land> R;
R.setZero();
R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
R(Y_land_vx, Y_land_vx) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get();
R(Y_land_vy, Y_land_vy) = _param_lpe_land_vxy.get() * _param_lpe_land_vxy.get();
R(Y_land_agl, Y_land_agl) = _param_lpe_land_z.get() * _param_lpe_land_z.get();
// residual
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
src/modules/local_position_estimator/sensors/landing_target.cpp
+6 -6
View File
@@ -8,7 +8,7 @@ static const uint64_t TARGET_TIMEOUT = 2000000; // [us]
void BlockLocalPositionEstimator::landingTargetInit()
{
if (_target_mode.get() == Target_Moving) {
if (_param_ltest_mode.get() == Target_Moving) {
// target is in moving mode, do not initialize
return;
}
@@ -24,7 +24,7 @@ void BlockLocalPositionEstimator::landingTargetInit()
int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target> &y)
{
if (_target_mode.get() == Target_Stationary) {
if (_param_ltest_mode.get() == Target_Stationary) {
if (_sub_landing_target_pose.get().rel_vel_valid) {
y(0) = _sub_landing_target_pose.get().vx_rel;
y(1) = _sub_landing_target_pose.get().vy_rel;
@@ -43,7 +43,7 @@ int BlockLocalPositionEstimator::landingTargetMeasure(Vector<float, n_y_target>
void BlockLocalPositionEstimator::landingTargetCorrect()
{
if (_target_mode.get() == Target_Moving) {
if (_param_ltest_mode.get() == Target_Moving) {
// nothing to do in this mode
return;
}
@@ -58,9 +58,9 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
float cov_vy = _sub_landing_target_pose.get().cov_vy_rel;
// use sensor value only if reasoanble
if (cov_vx < _target_min_cov.get() || cov_vy < _target_min_cov.get()) {
cov_vx = _target_min_cov.get();
cov_vy = _target_min_cov.get();
if (cov_vx < _param_lpe_lt_cov.get() || cov_vy < _param_lpe_lt_cov.get()) {
cov_vx = _param_lpe_lt_cov.get();
cov_vy = _param_lpe_lt_cov.get();
}
// target measurement matrix and noise matrix
src/modules/local_position_estimator/sensors/lidar.cpp
+2 -2
View File
@@ -53,7 +53,7 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
_time_last_lidar = _timeStamp;
y.setZero();
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
y(0) = (d + _lidar_z_offset.get()) *
y(0) = (d + _param_lpe_ldr_off_z.get()) *
cosf(euler.phi()) *
cosf(euler.theta());
return OK;
@@ -80,7 +80,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
float cov = _sub_lidar->get().variance;
if (cov < 1.0e-3f) {
R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get();
R(0, 0) = _param_lpe_ldr_z.get() * _param_lpe_ldr_z.get();
} else {
R(0, 0) = cov;
src/modules/local_position_estimator/sensors/mocap.cpp
+5 -5
View File
@@ -122,20 +122,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
R.setZero();
// use std dev from mocap data if available
if (_mocap_eph > _mocap_p_stddev.get()) {
if (_mocap_eph > _param_lpe_vic_p.get()) {
R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph;
R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph;
} else {
R(Y_mocap_x, Y_mocap_x) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
R(Y_mocap_y, Y_mocap_y) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
R(Y_mocap_x, Y_mocap_x) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
R(Y_mocap_y, Y_mocap_y) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
if (_mocap_epv > _mocap_p_stddev.get()) {
if (_mocap_epv > _param_lpe_vic_p.get()) {
R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv;
} else {
R(Y_mocap_z, Y_mocap_z) = _mocap_p_stddev.get() * _mocap_p_stddev.get();
R(Y_mocap_z, Y_mocap_z) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
// residual
src/modules/local_position_estimator/sensors/sonar.cpp
+2 -2
View File
@@ -67,7 +67,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
_time_last_sonar = _timeStamp;
y.setZero();
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
y(0) = (d + _sonar_z_offset.get()) *
y(0) = (d + _param_lpe_snr_off_z.get()) *
cosf(euler.phi()) *
cosf(euler.theta());
return OK;
@@ -90,7 +90,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
if (cov < 1.0e-3f) {
// use sensor value if reasoanble
cov = _sonar_z_stddev.get() * _sonar_z_stddev.get();
cov = _param_lpe_snr_z.get() * _param_lpe_snr_z.get();
}
// sonar measurement matrix and noise matrix
src/modules/local_position_estimator/sensors/vision.cpp
+6 -6
View File
@@ -127,20 +127,20 @@ void BlockLocalPositionEstimator::visionCorrect()
R.setZero();
// use std dev from vision data if available
if (_vision_eph > _vision_xy_stddev.get()) {
if (_vision_eph > _param_lpe_vis_xy.get()) {
R(Y_vision_x, Y_vision_x) = _vision_eph * _vision_eph;
R(Y_vision_y, Y_vision_y) = _vision_eph * _vision_eph;
} else {
R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
R(Y_vision_x, Y_vision_x) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get();
R(Y_vision_y, Y_vision_y) = _param_lpe_vis_xy.get() * _param_lpe_vis_xy.get();
}
if (_vision_epv > _vision_z_stddev.get()) {
if (_vision_epv > _param_lpe_vis_z.get()) {
R(Y_vision_z, Y_vision_z) = _vision_epv * _vision_epv;
} else {
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
R(Y_vision_z, Y_vision_z) = _param_lpe_vis_z.get() * _param_lpe_vis_z.get();
}
// vision delayed x
@@ -151,7 +151,7 @@ void BlockLocalPositionEstimator::visionCorrect()
if (vision_delay < 0.0f) { vision_delay = 0.0f; }
// use auto-calculated delay from measurement if parameter is set to zero
if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; }
if (getDelayPeriods(_param_lpe_vis_delay.get() > 0.0f ? _param_lpe_vis_delay.get() : vision_delay, &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);