coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

The weighting mechanism to only consider accelerations roughly

Browse Source 
in line with gravity wasn't working too well. Let's consider
all readings equally for now.
This commit is contained in:
Gavan Fantom
2014-05-17 12:35:20 +00:00
parent 9d5e6da3bf
commit 56de69735f
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
dcm.c
View File

@@ -173,11 +173,13 @@ bool dcm_renormalise(float *v)
void dcm_drift_correction(float x, float y, float z) void dcm_drift_correction(float x, float y, float z)
{ {
float mag;
float weight; float weight;
float error[3]; float error[3];
int i; int i;
#if DCM_WEIGHT
float mag;
mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY; mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;
mag = 1-mag; mag = 1-mag;
@@ -190,6 +192,9 @@ void dcm_drift_correction(float x, float y, float z)
weight = 0.0; weight = 0.0;
if (weight > 1.0) if (weight > 1.0)
weight = 1.0; weight = 1.0;
#else
weight = 1.0;
#endif
/* error = cross product of dcm last row and acceleration vector */ /* error = cross product of dcm last row and acceleration vector */
/* third row = cross product of first two rows */ /* third row = cross product of first two rows */