The unified diff between revisions [4f22e7ef..] and [9142f333..] 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 [9142f3330490a5aa00c1686475633b620c2ef5e7]
#
# add_file "sensors.c"
#  content [129fa873f84fea1677a1d27563afd5402f8d6677]
#
============================================================
--- /dev/null	
+++ sensors.c	129fa873f84fea1677a1d27563afd5402f8d6677
@@ -0,0 +1,233 @@
+/* sensors.c */
+
+#include "mpu6050.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_start_fns[])(void) = {
+	mpu6050_start_sample,
+};
+
+#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;
+
+float sensors_gyro_roll;
+float sensors_gyro_pitch;
+float sensors_gyro_yaw;
+
+float sensors_temp;
+
+float sensors_accel_x;
+float sensors_accel_y;
+float sensors_accel_z;
+
+float gyro_yaw_zero;
+float gyro_pitch_zero;
+float gyro_roll_zero;
+
+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)
+{
+	next_sensor = 0;
+
+	if (!mpu6050_init())
+		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(float roll, float pitch, float yaw)
+{
+#if 0
+	sensors_gyro_roll = roll - gyro_roll_zero;
+	sensors_gyro_pitch = pitch - gyro_pitch_zero;
+	sensors_gyro_yaw = yaw - gyro_yaw_zero;
+#else
+	sensors_gyro_roll = roll;
+	sensors_gyro_pitch = pitch;
+	sensors_gyro_yaw = yaw;
+#endif
+}
+
+void sensors_write_accel_data(float x, float y, float z)
+{
+	sensors_accel_x = x;
+	sensors_accel_y = y;
+	sensors_accel_z = z;
+}
+
+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_roll);
+	log_put_float(sensors_gyro_pitch);
+	log_put_float(sensors_gyro_yaw);
+	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
+		dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll,
+		    -sensors_gyro_yaw);
+#else
+		dcm_update(0.0, 0.0, 0.0);
+#endif
+		if (!status_armed()) {
+			if ( (abs(sensors_gyro_roll)  < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) &&
+			     (abs(sensors_gyro_yaw)   < 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_yaw_zero += sensors_gyro_yaw;
+			gyro_pitch_zero += sensors_gyro_pitch;
+			gyro_roll_zero += sensors_gyro_roll;
+			sensors_generation++;
+			if (sensors_generation >= GYRO_ZERO_COUNT) {
+				sensors_zero = FALSE;
+				sensors_update = TRUE;
+				sensors_generation = 0;
+				gyro_yaw_zero /= GYRO_ZERO_COUNT;
+				gyro_pitch_zero /= GYRO_ZERO_COUNT;
+				gyro_roll_zero /= GYRO_ZERO_COUNT;
+				putstr("Zero finished\r\n");
+				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
+			}
+		}
+	}
+	watchdog_kick(WATCHDOG_GYRO);
+
+#if 1
+	dcm_drift_correction(-sensors_accel_y, -sensors_accel_x,
+		-sensors_accel_z);
+#endif
+#if 0
+	dcm_drift_correction(sensors_accel_x, sensors_accel_y,
+		sensors_accel_z);
+#endif
+
+	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_roll  * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_gyro_pitch * 1000.0));
+	putstr(",");
+	putint_s((int)(sensors_gyro_yaw   * 1000.0));
+	putstr(")");
+
+	putstr("(");
+	putint_s((int)(sensors_temp  * 1000.0));
+	putstr(")\r\n");
+}