The unified diff between revisions [4cc7246c..] and [23a3e9a5..] is displayed below. It can also be downloaded as a raw diff.

This diff has been restricted to the following files: 'dcm.c'

#
# old_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631]
# new_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
#
# patch "dcm.c"
#  from [16b59bc009e1e04c2932e132fa351a06fa789e3a]
#    to [35a44d5b49a5c9bf56678ae7af0b611fde405621]
#
============================================================
--- dcm.c	16b59bc009e1e04c2932e132fa351a06fa789e3a
+++ dcm.c	35a44d5b49a5c9bf56678ae7af0b611fde405621
@@ -2,11 +2,20 @@
 
 #ifdef WE_HAVE_SQRT
 #include <math.h>
+#else
+#include "fisqrt.h"
 #endif
 #include "matrix.h"
 #include "dcm.h"
 #include "uart.h"
 
+#define GRAVITY 9.80665f
+
+#define KP_ROLLPITCH 0.05967
+#define KI_ROLLPITCH 0.00001278
+
+#define ERROR_LIMIT 1.17f
+
 /* Implementation of the DCM IMU concept as described by Premerlani
  * and Bizard
  */
@@ -15,10 +24,17 @@ float dcm[3*3] = {1, 0, 0,
 		  0, 1, 0,
 		  0, 0, 1};
 
+float omega_p[3] = {0.0, 0.0, 0.0};
+float omega_i[3] = {0.0, 0.0, 0.0};
+
 float delta_t = 0.01;
 
-void dcm_update(float omega_x, float omega_y, float omega_z)
+void dcm_update(float x, float y, float z)
 {
+	float omega_x = x + omega_i[0] + omega_p[0];
+	float omega_y = y + omega_i[1] + omega_p[1];
+	float omega_z = z + omega_i[2] + omega_p[2];
+
 	float tx = delta_t * omega_x;
 	float ty = delta_t * omega_y;
 	float tz = delta_t * omega_z;
@@ -34,6 +50,36 @@ void dcm_update(float omega_x, float ome
 	dcm_normalise();
 }
 
+void dcm_setvector(float x, float y, float z)
+{
+	/* We're given the Z axis */
+	dcm[6] = x;
+	dcm[7] = y;
+	dcm[8] = z;
+
+	/* Second row = cross product of unit X and third rows */
+	dcm[3] = 0.0;
+	dcm[4] = -dcm[8];
+	dcm[5] = dcm[7];
+
+	/* First row = cross product of third and second rows */
+	dcm[0] = dcm[7]*dcm[5] - dcm[8]*dcm[4];
+	dcm[1] = dcm[8]*dcm[3] - dcm[6]*dcm[5];
+	dcm[2] = dcm[6]*dcm[4] - dcm[7]*dcm[3];
+
+	/* Second row = cross product of third and first rows */
+	dcm[3] = dcm[7]*dcm[2] - dcm[8]*dcm[1];
+	dcm[4] = dcm[8]*dcm[0] - dcm[6]*dcm[2];
+	dcm[5] = dcm[6]*dcm[1] - dcm[7]*dcm[0];
+
+	dcm_renormalise(dcm+0);
+	dcm_renormalise(dcm+3);
+	dcm_renormalise(dcm+6);
+#if 0
+	dcm_normalise();
+#endif
+}
+
 void dcm_normalise(void)
 {
 	float error;
@@ -78,11 +124,14 @@ bool dcm_renormalise(float *v)
 
 	if (f < 1.5625f && f > 0.64f) {
 		f = 0.5 * (3 - f);
+	} else if (f < 100.0f && f > 0.01f) {
 #ifdef WE_HAVE_SQRT
-	} else if (f < 100.0f && f > 0.01f) {
 		f = 1.0 / sqrt(f);
-		/* XXX log this event? */
+#else
+		f = fisqrt(f);
 #endif
+		/* XXX log this event? */
+		putstr("sqrt\r\n");
 	} else {
 		putstr("problem\r\n");
 		return FALSE;
@@ -95,6 +144,64 @@ bool dcm_renormalise(float *v)
 	return TRUE;
 }
 
+void dcm_drift_correction(float x, float y, float z)
+{
+	float mag;
+	float weight;
+	float error[3];
+	int i;
+
+	mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;
+
+	mag = 1-mag;
+	if (mag < 0.0)
+		mag = -mag;
+
+	weight = 1 - 3*mag;
+
+	if (weight < 0.0)
+		weight = 0.0;
+	if (weight > 1.0)
+		weight = 1.0;
+
+	/* error = cross product of dcm last row and acceleration vector */
+	/* third row = cross product of first two rows */
+	error[0] = dcm[7]*z - dcm[8]*y;
+	error[1] = dcm[8]*x - dcm[6]*z;
+	error[2] = dcm[6]*y - dcm[7]*x;
+
+	for (i = 0; i < 3; i++) {
+		if (error[i] > ERROR_LIMIT)
+			error[i] = ERROR_LIMIT;
+		if (error[i] < -ERROR_LIMIT)
+			error[i] = -ERROR_LIMIT;
+	}
+
+	for (i = 0; i < 3; i++) {
+		omega_p[i] = error[i] * (KP_ROLLPITCH * weight);
+		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
+	}
+
+	putstr("w: ");
+	putint_s((int)(weight * 100000.0f));
+	putstr("\r\n");
+#if 0
+	putstr("p: ");
+	putint_s((int)(omega_p[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_p[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_p[2] * 100000.0f));
+	putstr(" i: ");
+	putint_s((int)(omega_i[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_i[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(omega_i[2] * 100000.0f));
+	putstr("\r\n");
+#endif
+}
+
 void dcm_dump(void)
 {
 	putstr("dcm: ");