From: Olivier Matz <zer0@droids-corp.org>
Date: Wed, 23 Jul 2014 21:46:42 +0000 (+0200)
Subject: allow to dump GPS info
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=66e8a69f26ffb8039688651235f223428383dac9;p=protos%2Fxbee-avr.git

allow to dump GPS info
---

diff --git a/commands.c b/commands.c
index befd1c9..9f7eaf6 100644
--- a/commands.c
+++ b/commands.c
@@ -48,6 +48,7 @@
 #include "main.h"
 #include "cmdline.h"
 #include "beep.h"
+#include "../fpv-common/i2c_commands.h"
 #include "i2c_protocol.h"
 #include "eeprom_config.h"
 
@@ -2247,31 +2248,47 @@ const parse_inst_t PROGMEM cmd_eeprom_list = {
 
 /* ************* */
 
-struct cmd_test_result {
+struct cmd_dump_i2c_result {
 	fixed_string_t cmd;
 };
 
-extern uint8_t imuboard; /* XXX test */
-static void cmd_test_parsed(void *parsed_result, void *data)
+static void cmd_dump_i2c_parsed(void *parsed_result, void *data)
 {
+	struct i2c_ans_imuboard_status imu;
+	uint8_t irq_flags;
+
 	(void)parsed_result;
 	(void)data;
-	printf("%d\n", imuboard);
-	i2c_protocol_debug();
+
+	while (!cmdline_keypressed()) {
+		IRQ_LOCK(irq_flags);
+		memcpy(&imu, &imuboard_status, sizeof(imu));
+		IRQ_UNLOCK(irq_flags);
+
+		if (imu.flags & IMUBOARD_STATUS_GPS_OK) {
+			printf_P(PSTR("GPS lat=%"PRIi32" long=%"PRIi32
+					" alt=%"PRIi32"\n"),
+				imu.latitude, imu.longitude, imu.altitude);
+		}
+		else
+			printf_P(PSTR("GPS unavailable"));
+		i2c_protocol_debug();
+		wait_ms(100);
+	}
 }
 
-const char PROGMEM str_test[] = "test";
-const parse_token_string_t PROGMEM cmd_test_cmd =
-	TOKEN_STRING_INITIALIZER(struct cmd_test_result, cmd,
-				 str_test);
+const char PROGMEM str_dump_i2c[] = "dump_i2c";
+const parse_token_string_t PROGMEM cmd_dump_i2c_cmd =
+	TOKEN_STRING_INITIALIZER(struct cmd_dump_i2c_result, cmd,
+				 str_dump_i2c);
 
-const char PROGMEM help_test[] = "test";
-const parse_inst_t PROGMEM cmd_test = {
-	.f = cmd_test_parsed,  /* function to call */
+const char PROGMEM help_dump_i2c[] = "dump_i2c";
+const parse_inst_t PROGMEM cmd_dump_i2c = {
+	.f = cmd_dump_i2c_parsed,  /* function to call */
 	.data = NULL,      /* 2nd arg of func */
-	.help_str = help_test,
+	.help_str = help_dump_i2c,
 	.tokens = {        /* token list, NULL terminated */
-		(PGM_P)&cmd_test_cmd,
+		(PGM_P)&cmd_dump_i2c_cmd,
 		NULL,
 	},
 };
@@ -2334,6 +2351,6 @@ const parse_ctx_t PROGMEM main_ctx[] = {
 	&cmd_eeprom_add,
 	&cmd_eeprom_add2,
 	&cmd_eeprom_list,
-	&cmd_test,
+	&cmd_dump_i2c,
 	NULL,
 };
diff --git a/i2c_protocol.c b/i2c_protocol.c
index 6734453..6b667b4 100644
--- a/i2c_protocol.c
+++ b/i2c_protocol.c
@@ -39,8 +39,6 @@
 #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 uint16_t i2c_errors = 0;
@@ -58,6 +56,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;
@@ -232,14 +233,14 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 	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));
 
 		break;
 	}
diff --git a/i2c_protocol.h b/i2c_protocol.h
index d101279..66d5831 100644
--- a/i2c_protocol.h
+++ b/i2c_protocol.h
@@ -22,17 +22,24 @@
 #ifndef _I2C_PROTOCOL_H_
 #define _I2C_PROTOCOL_H_
 
+/* load the timer for i2c protocol */
 void i2c_protocol_init(void);
+
+/* dump i2c debug infos (stats) */
 void i2c_protocol_debug(void);
 
+/* wait that all received status have been updated once */
 void i2cproto_wait_update(void);
 
-
 void i2c_recvevent(uint8_t *buf, int8_t size);
 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
 void i2c_sendevent(int8_t size);
 
+/* control the led of another board (debug) */
 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state);
 
+/* latest received imuboard_status. Access with care as it can be modified in
+ * the i2c timer. */
+struct i2c_ans_imuboard_status imuboard_status;
 
 #endif