fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix use of incorrect timestamp

Browse Source 
This was incorrectly using the IMU (1/250 sec) timestamp instead of the EKF (1/100 sec) value.
The corresponding accelerometer limit has been made a parameter and adjusted to match previous behaviour.
This commit is contained in:
Paul Riseborough 2017-02-27 15:46:39 +11:00 committed by Lorenz Meier
parent 879ad1fd2c
commit cf31945038
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/common.h
View File

@ -269,6 +269,9 @@ struct parameters {
float vel_Tau; // velocity state correction time constant (1/sec)
float pos_Tau; // postion state correction time constant (1/sec)
// state limits
float acc_bias_lim; // maximum accel bias magnitude (m/s/s)
unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected
@ -373,6 +376,9 @@ struct parameters {
vel_Tau = 0.25f;
pos_Tau = 0.25f;
// state limiting
acc_bias_lim = 0.4f;
no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps
}

4
EKF/ekf_helper.cpp
View File

@ -535,11 +535,11 @@ void Ekf::constrainStates()
}
for (int i = 0; i < 3; i++) {
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_imu_avg, 0.349066f * _dt_imu_avg);
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg);
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {