imu.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <math.h>
   4
   5 #include <aversive/irq_lock.h>
   6 #include <aversive/wait.h>
   7 #include <aversive/pgmspace.h>
   8
   9 #include <uart.h>
  10
  11 #include "mpu6050.h"
  12 #include "matrix.h"
  13 #include "MadgwickAHRS.h"
  14 #include "sd_log.h"
  15 #include "imu.h"
  16 #include "main.h"
  17
  18 /* structure storing the latest imu infos returned by the sensor */
  19 static struct imu_info g_imu;
  20
  21 /* structure storing the latest quaternion */
  22 static struct quaternion g_quat;
  23
  24 /* structure storing the latest euler position */
  25 static struct euler g_euler;
  26
  27 /* periodical timer structure */
  28 static struct callout imu_timer;
  29
  30 static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
  31 {
  32         float q0 = quat->q0;
  33         float q1 = quat->q1;
  34         float q2 = quat->q2;
  35         float q3 = quat->q3;
  36
  37         euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
  38                 q0*q0 - q1*q1 - q2*q2 + q3*q3);
  39         euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
  40         euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
  41                 q0*q0 + q1*q1 - q2*q2 - q3*q3);
  42 }
  43
  44 /* timer callback that polls IMU and does the calculation */
  45 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
  46 {
  47         struct imu_info imu;
  48         struct quaternion quat;
  49         struct euler euler;
  50         uint8_t irq_flags;
  51
  52         (void)arg;
  53
  54         /* copy previous quaternion locally */
  55         IRQ_LOCK(irq_flags);
  56         memcpy(&quat, &g_quat, sizeof(quat));
  57         IRQ_UNLOCK(irq_flags);
  58
  59         mpu6050_read_all_axes(&g_imu);
  60         MadgwickAHRSupdate(&g_imu, &g_quat);
  61         quaternion2euler(&g_quat, &g_euler);
  62
  63         /* update global variables */
  64         IRQ_LOCK(irq_flags);
  65         memcpy(&g_imu, &imu, sizeof(g_imu));
  66         IRQ_UNLOCK(irq_flags);
  67         IRQ_LOCK(irq_flags);
  68         memcpy(&g_quat, &quat, sizeof(g_quat));
  69         IRQ_UNLOCK(irq_flags);
  70         IRQ_LOCK(irq_flags);
  71         memcpy(&g_euler, &euler, sizeof(g_euler));
  72         IRQ_UNLOCK(irq_flags);
  73
  74         /* reschedule event */
  75         callout_schedule(cm, tim, 2);
  76 }
  77
  78 void imu_init(void)
  79 {
  80         mpu6050_init();
  81
  82         callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
  83         callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
  84 }
  85
  86 void imu_get_info(struct imu_info *imu)
  87 {
  88         memcpy(imu, &g_imu, sizeof(*imu));
  89 }
  90
  91 void imu_get_pos_quat(struct quaternion *quat)
  92 {
  93         memcpy(quat, &g_quat, sizeof(*quat));
  94 }
  95
  96 void imu_get_pos_euler(struct euler *euler)
  97 {
  98         memcpy(euler, &g_euler, sizeof(*euler));
  99 }
 100
 101
 102 int imu_log(void)
 103 {
 104         char buf[128];
 105         int16_t len;
 106         uint32_t ms;
 107         uint8_t flags;
 108         struct imu_info imu;
 109
 110         if (sd_log_enabled() == 0)
 111                 return 0;
 112
 113         IRQ_LOCK(flags);
 114         ms = global_ms;
 115         imu_get_info(&imu);
 116         IRQ_UNLOCK(flags);
 117
 118         len = snprintf(buf, sizeof(buf),
 119                 "%"PRIu32"\t"
 120                 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
 121                 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
 122                 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
 123                 ms,
 124                 imu.gx, imu.gy, imu.gz,
 125                 imu.ax, imu.ay, imu.az,
 126                 imu.mx, imu.my, imu.mz);
 127         if (sd_log_write(buf, len) != len) {
 128                 printf_P(PSTR("error writing to file\n"));
 129                 return -1;
 130         }
 131
 132         return 0;
 133 }