imuboard/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 = { 1.0, 0.0, 0.0, 0.0 };
  23
  24 /* structure storing the latest euler position */
  25 static struct euler g_euler;
  26 static struct imu_euler_int g_euler_int;
  27
  28 /* periodical timer structure */
  29 static struct callout imu_timer;
  30
  31 static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
  32 {
  33         float q0 = quat->q0;
  34         float q1 = quat->q1;
  35         float q2 = quat->q2;
  36         float q3 = quat->q3;
  37
  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);
  43 }
  44
  45 /* timer callback that polls IMU and does the calculation */
  46 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
  47 {
  48         struct imu_info imu;
  49         struct quaternion quat;
  50         struct euler euler;
  51         struct imu_euler_int euler_int;
  52         uint8_t irq_flags;
  53
  54         (void)arg;
  55
  56         /* copy previous quaternion locally */
  57         IRQ_LOCK(irq_flags);
  58         memcpy(&quat, &g_quat, sizeof(quat));
  59         IRQ_UNLOCK(irq_flags);
  60
  61         mpu6050_read_all_axes(&imu);
  62         MadgwickAHRSupdate(&imu, &quat);
  63         quaternion2euler(&quat, &euler);
  64
  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);
  68
  69         /* update global variables */
  70         IRQ_LOCK(irq_flags);
  71         memcpy(&g_imu, &imu, sizeof(g_imu));
  72         IRQ_UNLOCK(irq_flags);
  73         IRQ_LOCK(irq_flags);
  74         memcpy(&g_quat, &quat, sizeof(g_quat));
  75         IRQ_UNLOCK(irq_flags);
  76         IRQ_LOCK(irq_flags);
  77         memcpy(&g_euler, &euler, sizeof(g_euler));
  78         IRQ_UNLOCK(irq_flags);
  79         IRQ_LOCK(irq_flags);
  80         memcpy(&g_euler_int, &euler_int, sizeof(g_euler_int));
  81         IRQ_UNLOCK(irq_flags);
  82
  83         /* reschedule event */
  84         callout_schedule(cm, tim, 2);
  85 }
  86
  87 void imu_init(void)
  88 {
  89         printf_P(PSTR("imu_init\n"));
  90         mpu6050_init();
  91
  92         callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
  93         callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
  94 }
  95
  96 void imu_get_info(struct imu_info *imu)
  97 {
  98         memcpy(imu, &g_imu, sizeof(*imu));
  99 }
 100
 101 void imu_get_pos_quat(struct quaternion *quat)
 102 {
 103         memcpy(quat, &g_quat, sizeof(*quat));
 104 }
 105
 106 void imu_get_pos_euler(struct euler *euler)
 107 {
 108         memcpy(euler, &g_euler, sizeof(*euler));
 109 }
 110
 111 void imu_get_pos_euler_int(struct imu_euler_int *euler_int)
 112 {
 113         memcpy(euler_int, &g_euler_int, sizeof(*euler_int));
 114 }
 115
 116
 117 int imu_log(uint8_t to_stdout)
 118 {
 119         char buf[128];
 120         int16_t len;
 121         uint32_t ms;
 122         uint8_t flags;
 123         struct imu_info imu;
 124         struct euler angles;
 125
 126         IRQ_LOCK(flags);
 127         ms = global_ms;
 128         imu_get_info(&imu);
 129         IRQ_UNLOCK(flags);
 130
 131         IRQ_LOCK(flags);
 132         imu_get_pos_euler(&angles);
 133         IRQ_UNLOCK(flags);
 134
 135         len = snprintf(buf, sizeof(buf),
 136                 "%6.6"PRIu32" | "
 137                 "gyro %+2.2f %+2.2f %+2.2f | "
 138                 "accel %+2.2f %+2.2f %+2.2f | "
 139                 "magnet %+2.2f %+2.2f %+2.2f | "
 140                 "angles %+2.2f %+2.2f %+2.2f\r\n",
 141                 ms,
 142                 imu.gx, imu.gy, imu.gz,
 143                 imu.ax, imu.ay, imu.az,
 144                 imu.mx, imu.my, imu.mz,
 145                 angles.roll, angles.pitch, angles.yaw);
 146         if (!to_stdout && sd_log_enabled()) {
 147                 if (sd_log_write(buf, len) != len) {
 148                         printf_P(PSTR("error writing to file\n"));
 149                         return -1;
 150                 }
 151         }
 152         else if (to_stdout) {
 153                 printf_P(PSTR("%s"), buf);
 154         }
 155
 156         return 0;
 157 }