some cleaning of imu

[protos/imu.git] / imu.c

diff --git a/imu.c b/imu.c
index 0833a81..0c41556 100644 (file)
--- a/imu.c
+++ b/imu.c
@@ -8,7 +8,6 @@
 
 #include <uart.h>
 
-#include "i2c_helper.h"
 #include "mpu6050.h"
 #include "vector.h"
 #include "matrix.h"
@@ -17,40 +16,42 @@
 #include "imu.h"
 #include "main.h"
 
-static struct callout imu_timer;
+/* structure storing the latest imu infos returned by the sensor */
+static struct imu_info g_imu;
+
+/* structure storing the latest quaternion */
+static struct quaternion g_quat;
 
-double roll;
-double pitch;
-double yaw;
+/* structure storing the latest euler position */
+static struct euler g_euler;
+
+/* periodical timer structure */
+static struct callout imu_timer;
 
-void quaternion2euler(void)
+static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
 {
-       roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
-       pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
-       yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
+       float q0 = quat->q0;
+       float q1 = quat->q1;
+       float q2 = quat->q2;
+       float q3 = quat->q3;
+
+       euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
+               q0*q0 - q1*q1 - q2*q2 + q3*q3);
+       euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
+       euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
+               q0*q0 + q1*q1 - q2*q2 - q3*q3);
 }
 
+/* timer callback that polls IMU and does the calculation */
 static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 {
-       int16_t mpu6050_axes[10];
-
        (void)arg;
 
-       mpu6050_read_all_axes(mpu6050_axes);
-
-       MadgwickAHRSupdate(mpu6050_gx,
-               mpu6050_gy,
-               mpu6050_gz,
-               mpu6050_ax,
-               mpu6050_ay,
-               mpu6050_az,
-               mpu6050_mx,
-               mpu6050_my,
-               mpu6050_mz
-       );
-
-       quaternion2euler();
+       mpu6050_read_all_axes(&g_imu);
+       MadgwickAHRSupdate(&g_imu, &g_quat);
+       quaternion2euler(&g_quat, &g_euler);
 
+       /* reschedule event */
        callout_schedule(cm, tim, 2);
 }
 
@@ -64,20 +65,21 @@ void imu_init(void)
 
 void imu_get_info(struct imu_info *imu)
 {
-       imu->gx = mpu6050_gx;
-       imu->gy = mpu6050_gy;
-       imu->gz = mpu6050_gz;
-       imu->ax = mpu6050_ax;
-       imu->ay = mpu6050_ay;
-       imu->az = mpu6050_az;
-       imu->mx = mpu6050_mx;
-       imu->my = mpu6050_my;
-       imu->mz = mpu6050_mz;
-       imu->temp = 0;
+       memcpy(imu, &g_imu, sizeof(*imu));
 }
 
+void imu_get_pos_quat(struct quaternion *quat)
+{
+       memcpy(quat, &g_quat, sizeof(*quat));
+}
 
-int imu_loop(void)
+void imu_get_pos_euler(struct euler *euler)
+{
+       memcpy(euler, &g_euler, sizeof(*euler));
+}
+
+
+int imu_log(void)
 {
        char buf[128];
        int16_t len;
@@ -85,29 +87,26 @@ int imu_loop(void)
        uint8_t flags;
        struct imu_info imu;
 
-       while (1) {
-
-               IRQ_LOCK(flags);
-               ms = global_ms;
-               imu_get_info(&imu);
-               IRQ_UNLOCK(flags);
-
-               if (sd_log_enabled()) {
-                       len = snprintf(buf, sizeof(buf),
-                               "%"PRIu32"\t"
-                               "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
-                               "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
-                               "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
-                               ms,
-                               imu.gx, imu.gy, imu.gz,
-                               imu.ax, imu.ay, imu.az,
-                               imu.mx, imu.my, imu.mz);
-                       if (sd_log_write(buf, len) != len) {
-                               printf_P(PSTR("error writing to file\n"));
-                               return -1;
-                       }
-               }
-
+       if (sd_log_enabled() == 0)
+               return 0;
+
+       IRQ_LOCK(flags);
+       ms = global_ms;
+       imu_get_info(&imu);
+       IRQ_UNLOCK(flags);
+
+       len = snprintf(buf, sizeof(buf),
+               "%"PRIu32"\t"
+               "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
+               "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
+               "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
+               ms,
+               imu.gx, imu.gy, imu.gz,
+               imu.ax, imu.ay, imu.az,
+               imu.mx, imu.my, imu.mz);
+       if (sd_log_write(buf, len) != len) {
+               printf_P(PSTR("error writing to file\n"));
+               return -1;
        }
 
        return 0;