remove unused vector.[ch]
[protos/imu.git] / 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         (void)arg;
48
49         mpu6050_read_all_axes(&g_imu);
50         MadgwickAHRSupdate(&g_imu, &g_quat);
51         quaternion2euler(&g_quat, &g_euler);
52
53         /* reschedule event */
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         memcpy(imu, &g_imu, sizeof(*imu));
68 }
69
70 void imu_get_pos_quat(struct quaternion *quat)
71 {
72         memcpy(quat, &g_quat, sizeof(*quat));
73 }
74
75 void imu_get_pos_euler(struct euler *euler)
76 {
77         memcpy(euler, &g_euler, sizeof(*euler));
78 }
79
80
81 int imu_log(void)
82 {
83         char buf[128];
84         int16_t len;
85         uint32_t ms;
86         uint8_t flags;
87         struct imu_info imu;
88
89         if (sd_log_enabled() == 0)
90                 return 0;
91
92         IRQ_LOCK(flags);
93         ms = global_ms;
94         imu_get_info(&imu);
95         IRQ_UNLOCK(flags);
96
97         len = snprintf(buf, sizeof(buf),
98                 "%"PRIu32"\t"
99                 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
100                 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
101                 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
102                 ms,
103                 imu.gx, imu.gy, imu.gz,
104                 imu.ax, imu.ay, imu.az,
105                 imu.mx, imu.my, imu.mz);
106         if (sd_log_write(buf, len) != len) {
107                 printf_P(PSTR("error writing to file\n"));
108                 return -1;
109         }
110
111         return 0;
112 }