gps: dump position in i2c message

[protos/imu.git] / imu.c

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