dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: hotfix, change lower dt limit from 5 ms to 2 ms

This commit is contained in:
Anton Babushkin 2014-03-09 22:08:28 +04:00
parent 501dc0cfa7
commit 647142764f
2 changed files with 2 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/position_estimator_inav/inertial_filter.c
View File

@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
void inertial_filter_correct(float e, float dt, float x[3], int i, float w) void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{ {
float ewdt = w * dt; float ewdt = e * w * dt;
if (ewdt > 1.0f)
ewdt = 1.0f; // prevent over-correcting
ewdt *= e;
x[i] += ewdt; x[i] += ewdt;
if (i == 0) { if (i == 0) {

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.005); dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
t_prev = t; t_prev = t;
/* use GPS if it's valid and reference position initialized */ /* use GPS if it's valid and reference position initialized */