/* 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);
}