5 #include <aversive/irq_lock.h>
6 #include <aversive/wait.h>
7 #include <aversive/pgmspace.h>
13 #include "MadgwickAHRS.h"
18 /* structure storing the latest imu infos returned by the sensor */
19 static struct imu_info g_imu;
21 /* structure storing the latest quaternion */
22 static struct quaternion g_quat = { 1.0, 0.0, 0.0, 0.0 };
24 /* structure storing the latest euler position */
25 static struct euler g_euler;
26 static struct imu_euler_int g_euler_int;
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)
49 struct quaternion quat;
51 struct imu_euler_int euler_int;
56 /* copy previous quaternion locally */
58 memcpy(&quat, &g_quat, sizeof(quat));
59 IRQ_UNLOCK(irq_flags);
61 mpu6050_read_all_axes(&imu);
62 MadgwickAHRSupdate(&imu, &quat);
63 quaternion2euler(&quat, &euler);
65 euler_int.roll = euler.roll * (18000. / M_PI);
66 euler_int.pitch = euler.pitch * (18000. / M_PI);
67 euler_int.yaw = euler.yaw * (18000. / M_PI);
69 /* update global variables */
71 memcpy(&g_imu, &imu, sizeof(g_imu));
72 IRQ_UNLOCK(irq_flags);
74 memcpy(&g_quat, &quat, sizeof(g_quat));
75 IRQ_UNLOCK(irq_flags);
77 memcpy(&g_euler, &euler, sizeof(g_euler));
78 IRQ_UNLOCK(irq_flags);
80 memcpy(&g_euler_int, &euler_int, sizeof(g_euler_int));
81 IRQ_UNLOCK(irq_flags);
83 /* reschedule event */
84 callout_schedule(cm, tim, 2);
91 callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
92 callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
95 void imu_get_info(struct imu_info *imu)
97 memcpy(imu, &g_imu, sizeof(*imu));
100 void imu_get_pos_quat(struct quaternion *quat)
102 memcpy(quat, &g_quat, sizeof(*quat));
105 void imu_get_pos_euler(struct euler *euler)
107 memcpy(euler, &g_euler, sizeof(*euler));
110 void imu_get_pos_euler_int(struct imu_euler_int *euler_int)
112 memcpy(euler_int, &g_euler_int, sizeof(*euler_int));
116 int imu_log(uint8_t to_stdout)
131 imu_get_pos_euler(&angles);
134 len = snprintf(buf, sizeof(buf),
136 "gyro %+2.2f %+2.2f %+2.2f | "
137 "accel %+2.2f %+2.2f %+2.2f | "
138 "magnet %+2.2f %+2.2f %+2.2f | "
139 "angles %+2.2f %+2.2f %+2.2f\r\n",
141 imu.gx, imu.gy, imu.gz,
142 imu.ax, imu.ay, imu.az,
143 imu.mx, imu.my, imu.mz,
144 angles.roll, angles.pitch, angles.yaw);
145 if (!to_stdout && sd_log_enabled()) {
146 if (sd_log_write(buf, len) != len) {
147 printf_P(PSTR("error writing to file\n"));
151 else if (to_stdout) {
152 printf_P(PSTR("%s"), buf);