The unified diff between revisions [d0420ebd..] and [9142f333..] 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 [d0420ebd87c820e33a32b29727989516e15980a8]
# new_revision [9142f3330490a5aa00c1686475633b620c2ef5e7]
#
# patch "dcm.c"
#  from [34486e2e7948509c6f2648d365feb84c22ab5b14]
#    to [441585448f6476586c2fbcb260b7a6d908cab479]
#
============================================================
--- dcm.c	34486e2e7948509c6f2648d365feb84c22ab5b14
+++ dcm.c	441585448f6476586c2fbcb260b7a6d908cab479
@@ -9,14 +9,28 @@
 #include "dcm.h"
 #include "uart.h"
 #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
 
 #define ERROR_LIMIT 1.17f
 
+/* 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
  */
@@ -30,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];
@@ -51,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)
@@ -173,6 +197,15 @@ void dcm_drift_correction(float x, float
 	error[1] = dcm[8]*x - dcm[6]*z;
 	error[2] = dcm[6]*y - dcm[7]*x;
 
+	if (!status_armed()) {
+		if ((abs(error[0]) < ERROR_THRESHOLD) &&
+		    (abs(error[1]) < ERROR_THRESHOLD) &&
+		    (abs(error[2]) < ERROR_THRESHOLD))
+			status_set_ready(STATUS_MODULE_DCM_ERROR, TRUE);
+		else
+			status_set_ready(STATUS_MODULE_DCM_ERROR, FALSE);
+	}
+
 	for (i = 0; i < 3; i++) {
 		if (error[i] > ERROR_LIMIT)
 			error[i] = ERROR_LIMIT;
@@ -185,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));
@@ -208,6 +243,9 @@ void dcm_drift_correction(float x, float
 #endif
 }
 
+/* Maximum angle to the horizontal for arming: 30 degrees */
+#define ATTITUDE_THRESHOLD (0.5)
+
 void dcm_attitude_error(float roll, float pitch, float yaw)
 {
 	/* dcm[6] = sine of pitch */
@@ -221,6 +259,14 @@ void dcm_attitude_error(float roll, floa
 
 	/* TODO: What if we are upside down? */
 
+	if (!status_armed()) {
+		if ((abs(dcm[6]) < ATTITUDE_THRESHOLD) &&
+		    (abs(dcm[7]) < ATTITUDE_THRESHOLD))
+			status_set_ready(STATUS_MODULE_ATTITUDE, TRUE);
+		else
+			status_set_ready(STATUS_MODULE_ATTITUDE, FALSE);
+	}
+
 	motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
 }