X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=i2c_protocol.c;h=274e9f67d55fae1b104dc8f8cbc2591a5cd7ee6e;hp=f62bfb22a90845faade322c1e3d17a93e021a19a;hb=99a00646c85d806f34d7a4aa9f52b318607f7530;hpb=f8d665eef7b761e3b769af6b8dd9711754711cdd

diff --git a/i2c_protocol.c b/i2c_protocol.c
index f62bfb2..274e9f6 100644
--- 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)