diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-09 11:41:00 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-09 11:41:00 +0200 |
commit | b7d8d77c0f3b68430e9683316c41b0d9df4abd7d (patch) | |
tree | 82593402ed0b17d199d5e01949af6985d2d76803 /src | |
parent | 2fc011d20c7bd08bf5e076e79ba33a41f8fcbc30 (diff) | |
download | px4-firmware-b7d8d77c0f3b68430e9683316c41b0d9df4abd7d.tar.gz px4-firmware-b7d8d77c0f3b68430e9683316c41b0d9df4abd7d.tar.bz2 px4-firmware-b7d8d77c0f3b68430e9683316c41b0d9df4abd7d.zip |
position_estimator_inav: if flow is inaccurate, but used for correction with less weight
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 |
1 files changed, 4 insertions, 0 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d37c49696..506bc1bf0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -497,6 +497,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); flow_w = att.R[2][2] * flow_q_weight; + /* if flow is not accurate, lower weight for it */ + // TODO make this more fuzzy + if (!flow_accurate) + flow_w *= 0.2f; flow_valid = true; flow_valid_time = t; |