imuboard: fix reading and sending of imu position

[fpv.git] / imuboard / imu.c

diff --git a/imuboard/imu.c b/imuboard/imu.c
index 14f8479..00a61dd 100644 (file)
--- a/imuboard/imu.c
+++ b/imuboard/imu.c
@@ -19,10 +19,11 @@
 static struct imu_info g_imu;
 
 /* structure storing the latest quaternion */
-static struct quaternion g_quat;
+static struct quaternion g_quat = { 1.0, 0.0, 0.0, 0.0 };
 
 /* structure storing the latest euler position */
 static struct euler g_euler;
+static struct imu_euler_int g_euler_int;
 
 /* periodical timer structure */
 static struct callout imu_timer;
@@ -47,6 +48,7 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
        struct imu_info imu;
        struct quaternion quat;
        struct euler euler;
+       struct imu_euler_int euler_int;
        uint8_t irq_flags;
 
        (void)arg;
@@ -56,9 +58,13 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
        memcpy(&quat, &g_quat, sizeof(quat));
        IRQ_UNLOCK(irq_flags);
 
-       mpu6050_read_all_axes(&g_imu);
-       MadgwickAHRSupdate(&g_imu, &quat);
-       quaternion2euler(&quat, &g_euler);
+       mpu6050_read_all_axes(&imu);
+       MadgwickAHRSupdate(&imu, &quat);
+       quaternion2euler(&quat, &euler);
+
+       euler_int.roll = euler.roll * (18000. / M_PI);
+       euler_int.pitch = euler.pitch * (18000. / M_PI);
+       euler_int.yaw = euler.yaw * (18000. / M_PI);
 
        /* update global variables */
        IRQ_LOCK(irq_flags);
@@ -70,6 +76,9 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
        IRQ_LOCK(irq_flags);
        memcpy(&g_euler, &euler, sizeof(g_euler));
        IRQ_UNLOCK(irq_flags);
+       IRQ_LOCK(irq_flags);
+       memcpy(&g_euler_int, &euler_int, sizeof(g_euler_int));
+       IRQ_UNLOCK(irq_flags);
 
        /* reschedule event */
        callout_schedule(cm, tim, 2);
@@ -98,6 +107,10 @@ void imu_get_pos_euler(struct euler *euler)
        memcpy(euler, &g_euler, sizeof(*euler));
 }
 
+void imu_get_pos_euler_int(struct imu_euler_int *euler_int)
+{
+       memcpy(euler_int, &g_euler_int, sizeof(*euler_int));
+}
 
 
 int imu_log(uint8_t to_stdout)
@@ -107,24 +120,28 @@ int imu_log(uint8_t to_stdout)
        uint32_t ms;
        uint8_t flags;
        struct imu_info imu;
-
-       if (sd_log_enabled() == 0)
-               return 0;
+       struct euler angles;
 
        IRQ_LOCK(flags);
        ms = global_ms;
        imu_get_info(&imu);
        IRQ_UNLOCK(flags);
 
+       IRQ_LOCK(flags);
+       imu_get_pos_euler(&angles);
+       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",
+               "%6.6"PRIu32" | "
+               "gyro %+2.2f %+2.2f %+2.2f | "
+               "accel %+2.2f %+2.2f %+2.2f | "
+               "magnet %+2.2f %+2.2f %+2.2f | "
+               "angles %+2.2f %+2.2f %+2.2f\r\n",
                ms,
                imu.gx, imu.gy, imu.gz,
                imu.ax, imu.ay, imu.az,
-               imu.mx, imu.my, imu.mz);
+               imu.mx, imu.my, imu.mz,
+               angles.roll, angles.pitch, angles.yaw);
        if (!to_stdout && sd_log_enabled()) {
                if (sd_log_write(buf, len) != len) {
                        printf_P(PSTR("error writing to file\n"));