X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=i2c_protocol.c;h=ca4c49c8e9815a33fce30a45bf86e509b6981cb4;hp=67344537ee4d871f3958203f41905bf0d030190e;hb=HEAD;hpb=8f96511dd4a3a354a6a8c18bae5ade21b221fd95

diff --git a/i2c_protocol.c b/i2c_protocol.c
index 6734453..ca4c49c 100644
--- a/i2c_protocol.c
+++ b/i2c_protocol.c
@@ -30,8 +30,9 @@
 #include <i2c.h>
 
 #include "../fpv-common/i2c_commands.h"
-#include "main.h"
 #include "i2c_protocol.h"
+#include "beep.h"
+#include "main.h"
 
 #define I2C_STATE_MAX 2
 #define I2C_PERIOD_MS 50
@@ -39,12 +40,14 @@
 #define I2C_TIMEOUT 100 /* ms */
 #define I2C_MAX_ERRORS 40
 
-uint8_t imuboard = 0; /* XXX test */
-
 static volatile uint8_t i2c_poll_num = 0;
 static volatile uint8_t i2c_state = 0;
+static volatile uint8_t i2c_rx_count = 0;
+static volatile uint8_t i2c_tx_count = 0;
 static volatile uint16_t i2c_errors = 0;
 
+static uint8_t gps_ok = 0;
+
 #define OP_READY 0 /* no i2c op running */
 #define OP_POLL  1 /* a user command is running */
 #define OP_CMD   2 /* a polling (req / ans) is running */
@@ -58,6 +61,9 @@ static struct callout i2c_timer;
 
 static int8_t i2c_req_imuboard_status(void);
 
+/* latest received imuboard_status */
+struct i2c_ans_imuboard_status imuboard_status;
+
 /* used for commands */
 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 volatile int8_t command_dest=-1;
@@ -79,6 +85,8 @@ volatile uint8_t command_size=0;
 void i2c_protocol_debug(void)
 {
 	printf_P(PSTR("I2C protocol debug infos:\r\n"));
+	printf_P(PSTR("  i2c_send=%d\r\n"), i2c_tx_count);
+	printf_P(PSTR("  i2c_recv=%d\r\n"), i2c_rx_count);
 	printf_P(PSTR("  i2c_state=%d\r\n"), i2c_state);
 	printf_P(PSTR("  i2c_errors=%d\r\n"), i2c_errors);
 	printf_P(PSTR("  running_op=%d\r\n"), running_op);
@@ -197,6 +205,7 @@ void i2c_sendevent(int8_t size)
 		}
 		else
 			command_size = 0;
+		i2c_tx_count++;
 	}
 	else {
 		i2c_errors++;
@@ -229,17 +238,26 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 		goto error;
 	}
 
+	i2c_rx_count++;
+
 	switch (buf[0]) {
 
 	case I2C_ANS_IMUBOARD_STATUS: {
-		struct i2c_ans_imuboard_status * ans =
+		struct i2c_ans_imuboard_status *ans =
 			(struct i2c_ans_imuboard_status *)buf;
 
 		if (size != sizeof (*ans))
 			goto error;
 
-		/* status */
-		imuboard = ans->test;
+		/* copy status in a global struct */
+		memcpy(&imuboard_status, ans, sizeof(imuboard_status));
+
+		if (gps_ok == 0 &&
+			(imuboard_status.flags & IMUBOARD_STATUS_GPS_OK)) {
+			gps_ok = 1;
+			beep(0, 1, 1);
+			beep(0, 1, 1);
+		}
 
 		break;
 	}