5 #include <aversive/irq_lock.h>
6 #include <aversive/wait.h>
7 #include <aversive/pgmspace.h>
14 #include "MadgwickAHRS.h"
19 /* structure storing the latest imu infos returned by the sensor */
20 static struct imu_info g_imu;
22 /* structure storing the latest quaternion */
23 static struct quaternion g_quat;
25 /* structure storing the latest euler position */
26 static struct euler g_euler;
28 /* periodical timer structure */
29 static struct callout imu_timer;
31 static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
38 euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
39 q0*q0 - q1*q1 - q2*q2 + q3*q3);
40 euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
41 euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
42 q0*q0 + q1*q1 - q2*q2 - q3*q3);
45 /* timer callback that polls IMU and does the calculation */
46 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
50 mpu6050_read_all_axes(&g_imu);
51 MadgwickAHRSupdate(&g_imu, &g_quat);
52 quaternion2euler(&g_quat, &g_euler);
54 /* reschedule event */
55 callout_schedule(cm, tim, 2);
62 callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
63 callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
66 void imu_get_info(struct imu_info *imu)
68 memcpy(imu, &g_imu, sizeof(*imu));
71 void imu_get_pos_quat(struct quaternion *quat)
73 memcpy(quat, &g_quat, sizeof(*quat));
76 void imu_get_pos_euler(struct euler *euler)
78 memcpy(euler, &g_euler, sizeof(*euler));
90 if (sd_log_enabled() == 0)
98 len = snprintf(buf, sizeof(buf),
100 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
101 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
102 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
104 imu.gx, imu.gy, imu.gz,
105 imu.ax, imu.ay, imu.az,
106 imu.mx, imu.my, imu.mz);
107 if (sd_log_write(buf, len) != len) {
108 printf_P(PSTR("error writing to file\n"));