ViewMTN: Diff from [65df00aa..] to [64de686d..]

The unified diff between revisions [65df00aa..] and [64de686d..] 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 [65df00aa2705ce33fd74f4dd706d2879fe99b2b0]
# new_revision [64de686d701acb9539dc52fe0bff299405612ab0]
#
# add_file "dcm.c"
#  content [90d94d47a262c72d472ee7aca36bb55e2b328e66]
#
============================================================
--- /dev/null	
+++ dcm.c	90d94d47a262c72d472ee7aca36bb55e2b328e66
@@ -0,0 +1,308 @@
+/* dcm.c */
+
+#ifdef WE_HAVE_SQRT
+#include <math.h>
+#else
+#include "fisqrt.h"
+#endif
+#include "matrix.h"
+#include "dcm.h"
+#include "uart.h"
+#include "motor.h"
+#include "status.h"
+#include "abs.h"
+
+#define GRAVITY 9.80665f
+
+#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
+
+
+/* Implementation of the DCM IMU concept as described by Premerlani
+ * and Bizard
+ */
+
+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 omega_x, omega_y, omega_z;
+
+float delta_t = 0.01;
+
+void dcm_update(float x, float y, float z)
+{
+	omega_x = x + omega_i[0] + omega_p[0];
+	omega_y = y + omega_i[1] + omega_p[1];
+	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;
+
+	float update_matrix[3*3] = { 0, -tz,  ty,
+				    tz,   0, -tx,
+				   -ty,  tx,   0};
+	float temp_matrix[3*3];
+
+	matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3);
+	matrix_add(dcm, dcm, temp_matrix, 3, 3);
+
+	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;
+	float tmp[6];
+	int i;
+
+	/* dot product of first two rows */
+	error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5];
+
+	/* printf("error is %f\n", error); */
+
+	tmp[0] = dcm[3];
+	tmp[1] = dcm[4];
+	tmp[2] = dcm[5];
+	tmp[3] = dcm[0];
+	tmp[4] = dcm[1];
+	tmp[5] = dcm[2];
+
+	for (i = 0; i < 6; i++)
+		dcm[i] = dcm[i] - (tmp[i] * (0.5f * error));
+
+	/* third row = cross product of first two rows */
+	dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4];
+	dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5];
+	dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3];
+
+	if (!(dcm_renormalise(dcm+0) &&
+	      dcm_renormalise(dcm+3) &&
+	      dcm_renormalise(dcm+6))) {
+		/* Shit. I've been shot. */
+		dcm[0] = dcm[4] = dcm[8] = 1.0f;
+		dcm[1] = dcm[2] = dcm[3] = 0.0f;
+		dcm[5] = dcm[6] = dcm[7] = 0.0f;
+	}
+}
+
+bool dcm_renormalise(float *v)
+{
+	float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+
+	/* printf("f is %f\n", f); */
+
+	if (f < 1.5625f && f > 0.64f) {
+		f = 0.5 * (3 - f);
+	} else if (f < 100.0f && f > 0.01f) {
+#ifdef WE_HAVE_SQRT
+		f = 1.0 / sqrt(f);
+#else
+		f = fisqrt(f);
+#endif
+		/* XXX log this event? */
+		putstr("sqrt\r\n");
+	} else {
+		putstr("problem\r\n");
+		return FALSE;
+	}
+
+	v[0] = v[0] * f;
+	v[1] = v[1] * f;
+	v[2] = v[2] * f;
+
+	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;
+
+	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;
+		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);
+	}
+
+#if 0
+	putstr("w: ");
+	putint_s((int)(weight * 100000.0f));
+	putstr("\r\n");
+#endif
+
+#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
+}
+
+/* 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 */
+	/* dcm[7] = sine of roll */
+
+	/* pitch error = pitch - dcm[6] */
+	/* roll error = roll - dcm[7] */
+
+	/* That was the theory. In practice, there appears to be some
+	   confusion over axes. Pitch and roll seem.. reversed. */
+
+	/* 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);
+}
+
+void dcm_dump(void)
+{
+	putstr("dcm: ");
+	putint_s((int)(dcm[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[2] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[3] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[4] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[5] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[6] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[7] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[8] * 100000.0f));
+	putstr("\r\n");
+}
+
+void puthexfloat(float f)
+{
+	union {
+		float f;
+		unsigned int i;
+	} u;
+
+	u.f = f;
+	puthex(u.i);
+}
+
+void dcm_send_packet(void)
+{
+	putstr("D:(");
+	puthexfloat(dcm[0]);
+	putstr(",");
+	puthexfloat(dcm[1]);
+	putstr(",");
+	puthexfloat(dcm[2]);
+	putstr(",");
+	puthexfloat(dcm[3]);
+	putstr(",");
+	puthexfloat(dcm[4]);
+	putstr(",");
+	puthexfloat(dcm[5]);
+	putstr(",");
+	puthexfloat(dcm[6]);
+	putstr(",");
+	puthexfloat(dcm[7]);
+	putstr(",");
+	puthexfloat(dcm[8]);
+	putstr(")\r\n");
+}