X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=imu.c;h=0c41556aa35661133095e46ceae27f5b21f98588;hp=0833a8115dc3f12793f6a2d359c5844a3470a629;hb=2998e2d15b6098be4c272b187af9438d95b140cf;hpb=849b29b998a8cbc10cd1900d3e5dd1a8488b77f6

diff --git a/imu.c b/imu.c
index 0833a81..0c41556 100644
--- a/imu.c
+++ b/imu.c
@@ -8,7 +8,6 @@
 
 #include <uart.h>
 
-#include "i2c_helper.h"
 #include "mpu6050.h"
 #include "vector.h"
 #include "matrix.h"
@@ -17,40 +16,42 @@
 #include "imu.h"
 #include "main.h"
 
-static struct callout imu_timer;
+/* structure storing the latest imu infos returned by the sensor */
+static struct imu_info g_imu;
+
+/* structure storing the latest quaternion */
+static struct quaternion g_quat;
 
-double roll;
-double pitch;
-double yaw;
+/* structure storing the latest euler position */
+static struct euler g_euler;
+
+/* periodical timer structure */
+static struct callout imu_timer;
 
-void quaternion2euler(void)
+static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
 {
-	roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
-	pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
-	yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
+	float q0 = quat->q0;
+	float q1 = quat->q1;
+	float q2 = quat->q2;
+	float q3 = quat->q3;
+
+	euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
+		q0*q0 - q1*q1 - q2*q2 + q3*q3);
+	euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
+	euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
+		q0*q0 + q1*q1 - q2*q2 - q3*q3);
 }
 
+/* timer callback that polls IMU and does the calculation */
 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 {
-	int16_t mpu6050_axes[10];
-
 	(void)arg;
 
-	mpu6050_read_all_axes(mpu6050_axes);
-
-	MadgwickAHRSupdate(mpu6050_gx,
-		mpu6050_gy,
-		mpu6050_gz,
-		mpu6050_ax,
-		mpu6050_ay,
-		mpu6050_az,
-		mpu6050_mx,
-		mpu6050_my,
-		mpu6050_mz
-	);
-
-	quaternion2euler();
+	mpu6050_read_all_axes(&g_imu);
+	MadgwickAHRSupdate(&g_imu, &g_quat);
+	quaternion2euler(&g_quat, &g_euler);
 
+	/* reschedule event */
 	callout_schedule(cm, tim, 2);
 }
 
@@ -64,20 +65,21 @@ void imu_init(void)
 
 void imu_get_info(struct imu_info *imu)
 {
-	imu->gx = mpu6050_gx;
-	imu->gy = mpu6050_gy;
-	imu->gz = mpu6050_gz;
-	imu->ax = mpu6050_ax;
-	imu->ay = mpu6050_ay;
-	imu->az = mpu6050_az;
-	imu->mx = mpu6050_mx;
-	imu->my = mpu6050_my;
-	imu->mz = mpu6050_mz;
-	imu->temp = 0;
+	memcpy(imu, &g_imu, sizeof(*imu));
 }
 
+void imu_get_pos_quat(struct quaternion *quat)
+{
+	memcpy(quat, &g_quat, sizeof(*quat));
+}
 
-int imu_loop(void)
+void imu_get_pos_euler(struct euler *euler)
+{
+	memcpy(euler, &g_euler, sizeof(*euler));
+}
+
+
+int imu_log(void)
 {
 	char buf[128];
 	int16_t len;
@@ -85,29 +87,26 @@ int imu_loop(void)
 	uint8_t flags;
 	struct imu_info imu;
 
-	while (1) {
-
-		IRQ_LOCK(flags);
-		ms = global_ms;
-		imu_get_info(&imu);
-		IRQ_UNLOCK(flags);
-
-		if (sd_log_enabled()) {
-			len = snprintf(buf, sizeof(buf),
-				"%"PRIu32"\t"
-				"gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
-				"accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
-				"magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
-				ms,
-				imu.gx, imu.gy, imu.gz,
-				imu.ax, imu.ay, imu.az,
-				imu.mx, imu.my, imu.mz);
-			if (sd_log_write(buf, len) != len) {
-				printf_P(PSTR("error writing to file\n"));
-				return -1;
-			}
-		}
-
+	if (sd_log_enabled() == 0)
+		return 0;
+
+	IRQ_LOCK(flags);
+	ms = global_ms;
+	imu_get_info(&imu);
+	IRQ_UNLOCK(flags);
+
+	len = snprintf(buf, sizeof(buf),
+		"%"PRIu32"\t"
+		"gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
+		"accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
+		"magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
+		ms,
+		imu.gx, imu.gy, imu.gz,
+		imu.ax, imu.ay, imu.az,
+		imu.mx, imu.my, imu.mz);
+	if (sd_log_write(buf, len) != len) {
+		printf_P(PSTR("error writing to file\n"));
+		return -1;
 	}
 
 	return 0;