The unified diff between revisions [4f22e7ef..] and [9ca449dd..] 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 [9ca449dd7941ad52e33bdcb5c28b2ba35d54219a]
#
# patch "dcm.c"
#  from [90d94d47a262c72d472ee7aca36bb55e2b328e66]
#    to [441585448f6476586c2fbcb260b7a6d908cab479]
#
============================================================
--- dcm.c	90d94d47a262c72d472ee7aca36bb55e2b328e66
+++ dcm.c	441585448f6476586c2fbcb260b7a6d908cab479
@@ -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)
@@ -200,6 +218,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));