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

diff --git a/imu.c b/imu.c
index 0c41556..7e9a17f 100644
--- a/imu.c
+++ b/imu.c
@@ -9,7 +9,6 @@
 #include <uart.h>
 
 #include "mpu6050.h"
-#include "vector.h"
 #include "matrix.h"
 #include "MadgwickAHRS.h"
 #include "sd_log.h"
@@ -45,12 +44,33 @@ static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
 /* timer callback that polls IMU and does the calculation */
 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 {
+	struct imu_info imu;
+	struct quaternion quat;
+	struct euler euler;
+	uint8_t irq_flags;
+
 	(void)arg;
 
+	/* copy previous quaternion locally */
+	IRQ_LOCK(irq_flags);
+	memcpy(&quat, &g_quat, sizeof(quat));
+	IRQ_UNLOCK(irq_flags);
+
 	mpu6050_read_all_axes(&g_imu);
 	MadgwickAHRSupdate(&g_imu, &g_quat);
 	quaternion2euler(&g_quat, &g_euler);
 
+	/* update global variables */
+	IRQ_LOCK(irq_flags);
+	memcpy(&g_imu, &imu, sizeof(g_imu));
+	IRQ_UNLOCK(irq_flags);
+	IRQ_LOCK(irq_flags);
+	memcpy(&g_quat, &quat, sizeof(g_quat));
+	IRQ_UNLOCK(irq_flags);
+	IRQ_LOCK(irq_flags);
+	memcpy(&g_euler, &euler, sizeof(g_euler));
+	IRQ_UNLOCK(irq_flags);
+
 	/* reschedule event */
 	callout_schedule(cm, tim, 2);
 }