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

diff --git a/imu.c b/imu.c
index 86d36f2..7e9a17f 100644
--- a/imu.c
+++ b/imu.c
@@ -44,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);
 }