From b7d8d77c0f3b68430e9683316c41b0d9df4abd7d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Oct 2013 11:41:00 +0200 Subject: [PATCH] position_estimator_inav: if flow is inaccurate, but used for correction with less weight --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++++ 1 file changed, 4 insertions(+) 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 d37c49696d..506bc1bf05 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;