imuboard: fix reading and sending of imu position

diff --git a/common/i2c_commands.h b/common/i2c_commands.h
index 7a9b9c5154146bac7f09e49d191ee7da89da3d3b..165336833f48144c0c0d7cbb2d13c1644dcc74f2 100644 (file)
--- a/common/i2c_commands.h
+++ b/common/i2c_commands.h
@@ -70,9 +70,9 @@ struct i2c_ans_imuboard_status {
                            * positive means east */
        uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */
 
-       uint16_t roll;     /* XXX unit ? */
-       uint16_t pitch;
-       uint16_t yaw;
+       uint16_t roll;     /* 1/100 degrees */
+       uint16_t pitch;    /* 1/100 degrees */
+       uint16_t yaw;      /* 1/100 degrees */
 };
 
 #endif /* _I2C_PROTOCOL_H_ */

diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c
index 015e49c0af0c45443805f4b3dcc04e5bcd5b772d..3edb9b48e3534820596ef9e0facfa7db26bb06be 100644 (file)
--- a/imuboard/i2c_protocol.c
+++ b/imuboard/i2c_protocol.c
@@ -34,6 +34,7 @@
 
 #include "../common/i2c_commands.h"
 #include "gps_venus.h"
+#include "imu.h"
 #include "main.h"
 
 void i2c_protocol_init(void)
@@ -72,6 +73,7 @@ static void i2c_send_status(void)
 {
        struct i2c_ans_imuboard_status ans;
        struct gps_pos gps_pos;
+       struct imu_euler_int imu_pos;
        uint8_t irq_flags;
 
        i2c_flush();
@@ -82,6 +84,11 @@ static void i2c_send_status(void)
        gps_get_pos(&gps_pos);
        IRQ_UNLOCK(irq_flags);
 
+       /* imu */
+       IRQ_LOCK(irq_flags);
+       imu_get_pos_euler_int(&imu_pos);
+       IRQ_UNLOCK(irq_flags);
+
        ans.flags = 0;
        if (imuboard.flags & IMUBOARD_F_BOOT_OK)
                ans.flags |= I2C_IMUBOARD_STATUS_BOOT_OK;
@@ -95,6 +102,9 @@ static void i2c_send_status(void)
                ans.altitude = gps_pos.altitude;
        }
 
+       ans.roll = imu_pos.roll;
+       ans.pitch = imu_pos.pitch;
+       ans.yaw = imu_pos.yaw;
        i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
                        sizeof(ans), I2C_CTRL_GENERIC);
 }

diff --git a/imuboard/imu.c b/imuboard/imu.c
index 14f8479ac5959a5dc67b7235bb5e8d783c1b1dae..00a61dd898bca8975ce57af9e052d3c30a9b707b 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"));

diff --git a/imuboard/imu.h b/imuboard/imu.h
index 06755c9aee35e762f89d46f92f9166b8c043bdfe..d5455f463db1a4b2699db35880059f644e4d6d52 100644 (file)
--- a/imuboard/imu.h
+++ b/imuboard/imu.h
@@ -29,6 +29,9 @@
 #ifndef IMU_H_
 #define IMU_H_
 
+#include <math.h>
+#include <stdint.h>
+
 struct imu_info {
        /* gyro */
        double gx;
@@ -62,6 +65,13 @@ struct euler {
        double yaw;
 };
 
+/* angles in 1/100 degrees */
+struct imu_euler_int {
+       int16_t roll;
+       int16_t pitch;
+       int16_t yaw;
+};
+
 /* initialize the IMU */
 void imu_init(void);
 
@@ -72,12 +82,16 @@ int imu_log(uint8_t to_stdout);
  * the timer callback. Does not lock irq, so it's up to the user to do that. */
 void imu_get_info(struct imu_info *imu);
 
-/* return the latest position in a quaternion struct read in the timer
- * callback. Does not lock irq, so it's up to the user to do that. */
+/* return the latest position read in the timer callback in a quaternion
+ * struct. Does not lock irq, so it's up to the user to do that. */
 void imu_get_pos_quat(struct quaternion *pos);
 
-/* return the latest position in an euler struct read in the timer
- * callback. Does not lock irq, so it's up to the user to do that. */
+/* return the latest position read in the timer callback in an euler
+ * struct. Does not lock irq, so it's up to the user to do that. */
 void imu_get_pos_euler(struct euler *pos);
 
+/* return the latest position read in the timer callback in an imu_euler_int
+ * struct. Does not lock irq, so it's up to the user to do that. */
+void imu_get_pos_euler_int(struct imu_euler_int *pos);
+
 #endif