The unified diff between revisions [4f22e7ef..] and [08a35a66..] is displayed below. It can also be downloaded as a raw diff.

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

#
# old_revision [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b]
# new_revision [08a35a6680cdf8985cfb16fa6779ee6db7202a9c]
#
# add_file "sensors.c"
#  content [178f9bbaac429772d293d7749bb23c32fbed0c98]
#
============================================================
--- /dev/null	
+++ sensors.c	178f9bbaac429772d293d7749bb23c32fbed0c98
@@ -0,0 +1,238 @@
+/* sensors.c */
+
+#include "mpu6050.h"
+#include "hmc5883l.h"
+#include "mpl3115a2.h"
+#include "dcm.h"
+#include "fisqrt.h"
+#include "watchdog.h"
+#include "status.h"
+#include "abs.h"
+#include "panic.h"
+#include "uart.h"
+#include "log.h"
+#include "stick.h"
+
+bool (*sensor_init_fns[])(void) = {
+	mpu6050_init,
+	hmc5883l_init,
+	mpl3115a2_init,
+};
+
+bool (*sensor_start_fns[])(void) = {
+	mpu6050_start_sample,
+	hmc5883l_start_sample,
+	mpl3115a2_start_sample,
+};
+
+#define SENSOR_INIT_FNS (sizeof(sensor_init_fns)/sizeof(sensor_init_fns[0]))
+#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))
+
+static unsigned int next_sensor;
+
+static bool sensors_zero;
+static bool sensors_update;
+static unsigned int sensors_discard;
+static unsigned int sensors_generation;
+
+/* x = roll
+   y = pitch
+   z = yaw
+ */
+vec3f sensors_gyro;
+vec3f gyro_zero;
+
+float sensors_temp;
+
+vec3f sensors_accel;
+
+void sensors_write_log(void);
+void sensors_process(void);
+
+#define TWO_PI 6.28318531f
+#define DEG_TO_RAD (TWO_PI/360.0f)
+
+/* The gyro has to stay within this limit in each axis in order to arm */
+#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD)
+
+#define GYRO_ZERO_COUNT 1000
+
+void sensors_dump(void);
+
+bool sensors_init(void)
+{
+	unsigned int i;
+
+	next_sensor = 0;
+
+	for (i = 0; i < SENSOR_INIT_FNS; i++)
+		if (!(sensor_init_fns[i])())
+			return FALSE;
+
+	return TRUE;
+}
+
+bool sensors_next_sample(void)
+{
+	bool result;
+
+	result = (sensor_start_fns[next_sensor++])();
+	if (next_sensor >= SENSOR_START_FNS)
+		next_sensor = 0;
+
+	return result;
+}
+
+void sensors_sample_done(void)
+{
+	if (next_sensor == 0) {
+		sensors_write_log();
+		return;
+	}
+
+	if (!sensors_next_sample())
+		panic(PANIC_SENSOR_FAIL);
+}
+
+bool sensors_start_sample(void)
+{
+	next_sensor = 0;
+	return sensors_next_sample();
+}
+
+void sensors_write_gyro_data(vec3f gyro)
+{
+#if 0
+	sensors_gyro.x = gyro.x - gyro_zero.x;
+	sensors_gyro.y = gyro.y - gyro_zero.y;
+	sensors_gyro.z = gyro.z - gyro_zero.z;
+#else
+	sensors_gyro = gyro;
+#endif
+}
+
+void sensors_write_accel_data(vec3f accel)
+{
+	sensors_accel = accel;
+}
+
+void sensors_write_temp_data(float temp)
+{
+	sensors_temp = temp;
+	/* XXX HACK find a better place for this call */
+	sensors_process();
+}
+
+#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A
+#define LOG_SIGNATURE_SENSORS2 0xDA7AF173
+void sensors_write_log(void)
+{
+#if 0
+	log_put_uint(LOG_SIGNATURE_SENSORS);
+	log_put_float(sensors_accel.x);
+	log_put_float(sensors_accel.y);
+	log_put_float(sensors_accel.z);
+	log_put_float(sensors_gyro.x);
+	log_put_float(sensors_gyro.y);
+	log_put_float(sensors_gyro.z);
+	log_put_float(sensors_temp);
+#else
+	/* XXX this just about comes out in the right place, but by luck */
+	log_put_uint(LOG_SIGNATURE_SENSORS2);
+#endif
+}
+
+void sensors_start_zero(void)
+{
+	sensors_zero = TRUE;
+	sensors_update = FALSE;
+	sensors_discard = 100;
+	sensors_generation = 0;
+	putstr("Starting zero\r\n");
+	mpu6050_start_zero();
+}
+
+void sensors_process(void)
+{
+	if (sensors_update) {
+#if 1
+		vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z};
+#else
+		vec3f dcm_gyro = {0.0, 0.0, 0.0};
+#endif
+		dcm_update(dcm_gyro);
+		if (!status_armed()) {
+			if ( (abs(sensors_gyro.x)  < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro.y)  < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro.z)  < GYRO_RATE_THRESHOLD)) {
+			    status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
+			} else {
+			    status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
+			}
+		}
+
+		sensors_generation++;
+
+#if SEND_DCM
+		if ((sensors_generation % 40) == 0) {
+			dcm_send_packet();
+			sensors_dump();
+		}
+#endif
+
+	} else if (sensors_zero) {
+		if (sensors_discard) {
+			sensors_discard--;
+		} else {
+			gyro_zero.z += sensors_gyro.z;
+			gyro_zero.y += sensors_gyro.y;
+			gyro_zero.x += sensors_gyro.x;
+			sensors_generation++;
+			if (sensors_generation >= GYRO_ZERO_COUNT) {
+				sensors_zero = FALSE;
+				sensors_update = TRUE;
+				sensors_generation = 0;
+				gyro_zero.z /= GYRO_ZERO_COUNT;
+				gyro_zero.y /= GYRO_ZERO_COUNT;
+				gyro_zero.x /= GYRO_ZERO_COUNT;
+				putstr("Zero finished\r\n");
+				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
+			}
+		}
+	}
+	watchdog_kick(WATCHDOG_GYRO);
+
+#if 1
+	vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z};
+#endif
+#if 0
+	vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z};
+#endif
+	dcm_drift_correction(dcm_accel);
+
+	watchdog_kick(WATCHDOG_ACCEL);
+	stick_input();
+}
+
+void sensors_dump(void)
+{
+	putstr("(");
+	putint_s((int)(sensors_accel.x * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_accel.y * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_accel.z * 1000.0));
+	putstr(")");
+
+	putstr("(");
+	putint_s((int)(sensors_gyro.x * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_gyro.y * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_gyro.z * 1000.0));
+	putstr(")");
+
+	putstr("(");
+	putint_s((int)(sensors_temp  * 1000.0));
+	putstr(")\r\n");
+}