The unified diff between revisions [4f22e7ef..] and [bfc9e27f..] 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 [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b]
# new_revision [bfc9e27f5c40da31ae4269394aaf0545e5856a70]
#
# patch "dcm.c"
#  from [90d94d47a262c72d472ee7aca36bb55e2b328e66]
#    to [62d414448bb944679d6e5a3e693736501a28a009]
#
============================================================
--- dcm.c	90d94d47a262c72d472ee7aca36bb55e2b328e66
+++ dcm.c	62d414448bb944679d6e5a3e693736501a28a009
@@ -11,9 +11,14 @@
 #include "motor.h"
 #include "status.h"
 #include "abs.h"
+#include "log.h"
 
+#if 0
 #define GRAVITY 9.80665f
+#endif
 
+#define GRAVITY 1.0f
+
 #define KP_ROLLPITCH 0.05967
 #define KI_ROLLPITCH 0.00001278
 
@@ -22,7 +27,10 @@
 /* Maximum allowed error for arming */
 #define ERROR_THRESHOLD 0.20f
 
+#define LOG_MAGIC_DCM_UPDATE 0x00DC111A
+#define LOG_MAGIC_DCM_DRIFT 0x00DC111B
 
+
 /* Implementation of the DCM IMU concept as described by Premerlani
  * and Bizard
  */
@@ -36,8 +44,16 @@ float omega_x, omega_y, omega_z;
 
 float omega_x, omega_y, omega_z;
 
-float delta_t = 0.01;
+float delta_t = 0.005;
 
+void dcm_log(unsigned int magic)
+{
+	int i;
+	log_put_uint(magic);
+	for (i = 0; i < 9; i++)
+		log_put_float(dcm[i]);
+}
+
 void dcm_update(float x, float y, float z)
 {
 	omega_x = x + omega_i[0] + omega_p[0];
@@ -57,6 +73,8 @@ void dcm_update(float x, float y, float 
 	matrix_add(dcm, dcm, temp_matrix, 3, 3);
 
 	dcm_normalise();
+
+/*	dcm_log(LOG_MAGIC_DCM_UPDATE); */
 }
 
 void dcm_setvector(float x, float y, float z)
@@ -155,11 +173,13 @@ void dcm_drift_correction(float x, float
 
 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;
@@ -172,6 +192,9 @@ void dcm_drift_correction(float x, float
 		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 */
@@ -200,6 +223,8 @@ void dcm_drift_correction(float x, float
 		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
 	}
 
+	dcm_log(LOG_MAGIC_DCM_DRIFT);
+
 #if 0
 	putstr("w: ");
 	putint_s((int)(weight * 100000.0f));