From 66e8a69f26ffb8039688651235f223428383dac9 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Wed, 23 Jul 2014 23:46:42 +0200 Subject: [PATCH] allow to dump GPS info --- commands.c | 47 ++++++++++++++++++++++++++++++++--------------- i2c_protocol.c | 11 ++++++----- i2c_protocol.h | 9 ++++++++- 3 files changed, 46 insertions(+), 21 deletions(-) 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 -- 2.20.1