gps: dump position in i2c message

[protos/imu.git] / i2c_protocol.c

diff --git a/i2c_protocol.c b/i2c_protocol.c
index f62bfb2..274e9f6 100644 (file)
--- a/i2c_protocol.c
+++ b/i2c_protocol.c
@@ -33,6 +33,7 @@
 #include <i2c.h>
 
 #include "../fpv-common/i2c_commands.h"
+#include "gps_venus.h"
 #include "main.h"
 
 void i2c_protocol_init(void)
@@ -70,25 +71,26 @@ static void i2c_test(uint16_t val)
 static void i2c_send_status(void)
 {
        struct i2c_ans_imuboard_status ans;
-       static uint8_t x;
+       struct gps_pos gps_pos;
+       uint8_t irq_flags;
 
        i2c_flush();
        ans.hdr.cmd =  I2C_ANS_IMUBOARD_STATUS;
 
-       /* status */
-       ans.test = x++; /* XXX */
-#if 0
-       ans.mode = state_get_mode();
-       ans.status = state_get_status();
+       /* gps */
+       IRQ_LOCK(irq_flags);
+       gps_get_pos(&gps_pos);
+       IRQ_UNLOCK(irq_flags);
 
-       ans.left_cobroller_speed = imuboard.left_cobroller_speed;
-       ans.right_cobroller_speed = imuboard.right_cobroller_speed;
-
-       ans.cob_count = state_get_cob_count();
-#endif
+       if (gps_pos_valid(&gps_pos)) {
+               ans.flags |= IMUBOARD_STATUS_GPS_OK;
+               ans.latitude = gps_pos.latitude;
+               ans.longitude = gps_pos.longitude;
+               ans.altitude = gps_pos.altitude;
+       }
 
        i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
-                sizeof(ans), I2C_CTRL_GENERIC);
+                       sizeof(ans), I2C_CTRL_GENERIC);
 }
 
 void i2c_recvevent(uint8_t * buf, int8_t size)