gps_venus: most functions should be static

[fpv.git] / imuboard / i2c_protocol.c

diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c
index 9d73252..7d5a459 100644 (file)
--- a/imuboard/i2c_protocol.c
+++ b/imuboard/i2c_protocol.c
@@ -33,7 +33,8 @@
 #include <i2c.h>
 
 #include "../common/i2c_commands.h"
-#include "gps_venus.h"
+#include "gps.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,7 +84,17 @@ 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;
+       if (imuboard.flags & IMUBOARD_F_SDCARD_OK)
+               ans.flags |= I2C_IMUBOARD_STATUS_SDCARD_OK;
+
        if (gps_pos_valid(&gps_pos)) {
                ans.flags |= I2C_IMUBOARD_STATUS_GPS_OK;
                ans.latitude = gps_pos.latitude;
@@ -90,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);
 }