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 "i2c_helper.h"
  12 #include "mpu6050.h"
  13 #include "vector.h"
  14 #include "matrix.h"
  15 #include "MadgwickAHRS.h"
  16 #include "sd_log.h"
  17 #include "imu.h"
  18 #include "main.h"
  19
  20 static struct callout imu_timer;
  21
  22 double roll;
  23 double pitch;
  24 double yaw;
  25
  26 void quaternion2euler(void)
  27 {
  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);
  31 }
  32
  33 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
  34 {
  35         int16_t mpu6050_axes[10];
  36
  37         (void)arg;
  38
  39         mpu6050_read_all_axes(mpu6050_axes);
  40
  41         MadgwickAHRSupdate(mpu6050_gx,
  42                 mpu6050_gy,
  43                 mpu6050_gz,
  44                 mpu6050_ax,
  45                 mpu6050_ay,
  46                 mpu6050_az,
  47                 mpu6050_mx,
  48                 mpu6050_my,
  49                 mpu6050_mz
  50         );
  51
  52         quaternion2euler();
  53
  54         callout_schedule(cm, tim, 2);
  55 }
  56
  57 void imu_init(void)
  58 {
  59         mpu6050_init();
  60
  61         callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
  62         callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
  63 }
  64
  65 void imu_get_info(struct imu_info *imu)
  66 {
  67         imu->gx = mpu6050_gx;
  68         imu->gy = mpu6050_gy;
  69         imu->gz = mpu6050_gz;
  70         imu->ax = mpu6050_ax;
  71         imu->ay = mpu6050_ay;
  72         imu->az = mpu6050_az;
  73         imu->mx = mpu6050_mx;
  74         imu->my = mpu6050_my;
  75         imu->mz = mpu6050_mz;
  76         imu->temp = 0;
  77 }
  78
  79
  80 int imu_loop(void)
  81 {
  82         char buf[128];
  83         int16_t len;
  84         uint32_t ms;
  85         uint8_t flags;
  86         struct imu_info imu;
  87
  88         while (1) {
  89
  90                 IRQ_LOCK(flags);
  91                 ms = global_ms;
  92                 imu_get_info(&imu);
  93                 IRQ_UNLOCK(flags);
  94
  95                 if (sd_log_enabled()) {
  96                         len = snprintf(buf, sizeof(buf),
  97                                 "%"PRIu32"\t"
  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",
 101                                 ms,
 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"));
 107                                 return -1;
 108                         }
 109                 }
 110
 111         }
 112
 113         return 0;
 114 }