From 56de69735fd52f41a19f2ddf7be7c84f9ca20817 Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Sat, 17 May 2014 12:35:20 +0000 Subject: [PATCH] The weighting mechanism to only consider accelerations roughly in line with gravity wasn't working too well. Let's consider all readings equally for now. --- dcm.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dcm.c b/dcm.c index c3b6afe..32f0974 100644 --- a/dcm.c +++ b/dcm.c @@ -173,11 +173,13 @@ bool dcm_renormalise(float *v) void dcm_drift_correction(float x, float y, float z) { - float mag; float weight; float error[3]; int i; +#if DCM_WEIGHT + float mag; + mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY; mag = 1-mag; @@ -190,6 +192,9 @@ void dcm_drift_correction(float x, float y, float z) weight = 0.0; if (weight > 1.0) weight = 1.0; +#else + weight = 1.0; +#endif /* error = cross product of dcm last row and acceleration vector */ /* third row = cross product of first two rows */