5 #include <aversive/irq_lock.h>
6 #include <aversive/wait.h>
7 #include <aversive/pgmspace.h>
11 #include "i2c_helper.h"
15 #include "MadgwickAHRS.h"
20 static struct callout imu_timer;
26 void quaternion2euler(void)
28 roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
29 pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
30 yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
33 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
35 int16_t mpu6050_axes[10];
39 mpu6050_read_all_axes(mpu6050_axes);
41 MadgwickAHRSupdate(mpu6050_gx,
54 callout_schedule(cm, tim, 2);
61 callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
62 callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
65 void imu_get_info(struct imu_info *imu)
95 if (sd_log_enabled()) {
96 len = snprintf(buf, sizeof(buf),
98 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
99 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
100 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
102 imu.gx, imu.gy, imu.gz,
103 imu.ax, imu.ay, imu.az,
104 imu.mx, imu.my, imu.mz);
105 if (sd_log_write(buf, len) != len) {
106 printf_P(PSTR("error writing to file\n"));