X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=commands.c;h=d67b6515ba1bc60b88620f9c798c9f030f399f3c;hp=c6a418995582922e5f840797eb43476d825b1d73;hb=HEAD;hpb=13e5a26a5242e90e77786b58b5df15d77b7c340c

diff --git a/commands.c b/commands.c
index c6a4189..d67b651 100644
--- a/commands.c
+++ b/commands.c
@@ -48,6 +48,8 @@
 #include "main.h"
 #include "cmdline.h"
 #include "beep.h"
+#include "../fpv-common/i2c_commands.h"
+#include "i2c_protocol.h"
 #include "eeprom_config.h"
 
 /* commands_gen.c */
@@ -133,7 +135,7 @@ static void range_cb(struct callout_mgr *cm,
 static int8_t send_msg_cb(int8_t retcode, void *frame, unsigned len,
 	void *arg)
 {
-	struct xbee_recv_hdr *recvframe = frame;
+	struct xbee_xmit_status_hdr *recvframe = frame;
 	uint8_t *done = arg;
 
 	*done = 1;
@@ -142,7 +144,7 @@ static int8_t send_msg_cb(int8_t retcode, void *frame, unsigned len,
 		return retcode;
 	}
 	if (retcode == XBEE_USER_RETCODE_BAD_FRAME ||
-		len <  sizeof(*recvframe)) {
+		len != sizeof(*recvframe)) {
 		printf_P(PSTR("invalid frame\r\n"));
 		return XBEE_USER_RETCODE_BAD_FRAME;
 	}
@@ -1613,6 +1615,238 @@ const parse_inst_t PROGMEM cmd_rc_proto_stats = {
 
 /**********************************************************/
 
+/* this structure is filled when cmd_rc_proto_timers is parsed successfully */
+struct cmd_rc_proto_timers_result {
+	fixed_string_t arg0;
+	fixed_string_t arg1;
+	uint16_t servo_min;
+	uint16_t servo_max;
+	uint16_t power_probe;
+	uint16_t autobypass;
+};
+
+static void cmd_rc_proto_timers_parsed(void *parsed_result, void *data)
+{
+	struct cmd_rc_proto_timers_result *res = parsed_result;
+	(void)data;
+
+	if (!strcmp_P(res->arg1, PSTR("set"))) {
+		rc_proto_timers.send_servo_min_ms = res->servo_min;
+		rc_proto_timers.send_servo_max_ms = res->servo_max;
+		rc_proto_timers.send_power_probe_ms = res->power_probe;
+		rc_proto_timers.autobypass_ms = res->autobypass;
+	}
+
+	printf_P(PSTR("rc_proto_timers: min=%d, max=%d, "
+			"power_probe=%d autobypass=%d\n"),
+		rc_proto_timers.send_servo_min_ms,
+		rc_proto_timers.send_servo_max_ms,
+		rc_proto_timers.send_power_probe_ms,
+		rc_proto_timers.autobypass_ms);
+}
+
+const char PROGMEM str_rc_proto_timers_arg0[] = "rc_proto_timers";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_arg0 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg0,
+				 str_rc_proto_timers_arg0);
+const char PROGMEM str_rc_proto_timers_arg1[] = "set";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_arg1 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg1,
+				 str_rc_proto_timers_arg1);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_min =
+	TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, servo_min,
+		UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_max =
+	TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, servo_max,
+		UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_power_probe =
+	TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, power_probe,
+		UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_autobypass =
+	TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, autobypass,
+		UINT16);
+
+const char PROGMEM help_rc_proto_timers[] = "set rc_proto_timers (servo_min, "
+	"servo_max, pow_probe, autobypass)";
+const parse_inst_t PROGMEM cmd_rc_proto_timers = {
+	.f = cmd_rc_proto_timers_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_rc_proto_timers,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_rc_proto_timers_arg0,
+		(PGM_P)&cmd_rc_proto_timers_arg1,
+		(PGM_P)&cmd_rc_proto_timers_servo_min,
+		(PGM_P)&cmd_rc_proto_timers_servo_max,
+		(PGM_P)&cmd_rc_proto_timers_power_probe,
+		(PGM_P)&cmd_rc_proto_timers_autobypass,
+		NULL,
+	},
+};
+
+const char PROGMEM str_rc_proto_timers_show_arg1[] = "show";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_show_arg1 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg1,
+				 str_rc_proto_timers_show_arg1);
+
+const char PROGMEM help_rc_proto_timers_show[] = "show rc_proto timers value";
+const parse_inst_t PROGMEM cmd_rc_proto_timers_show = {
+	.f = cmd_rc_proto_timers_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_rc_proto_timers_show,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_rc_proto_timers_arg0,
+		(PGM_P)&cmd_rc_proto_timers_show_arg1,
+		NULL,
+	},
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_mode is parsed successfully */
+struct cmd_rc_proto_mode_result {
+	fixed_string_t arg0;
+	fixed_string_t cmd;
+	fixed_string_t val;
+};
+
+static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data)
+{
+	struct cmd_rc_proto_mode_result *res = parsed_result;
+	(void)data;
+	uint8_t flags;
+	uint8_t on = 0;
+
+	flags = rc_proto_get_mode();
+	if (!strcmp_P(res->val, PSTR("on")))
+		on = 1;
+
+	if (!strcmp_P(res->cmd, PSTR("rx_copy_spi"))) {
+		if (on == 1)
+			flags |= RC_PROTO_FLAGS_RX_COPY_SPI;
+		else
+			flags &= ~RC_PROTO_FLAGS_RX_COPY_SPI;
+	}
+	else if (!strcmp_P(res->cmd, PSTR("rx_autobypass"))) {
+		if (on == 1)
+			flags |= RC_PROTO_FLAGS_RX_AUTOBYPASS;
+		else
+			flags &= ~RC_PROTO_FLAGS_RX_AUTOBYPASS;
+	}
+	else if (!strcmp_P(res->cmd, PSTR("tx_stats"))) {
+		if (on == 1)
+			flags |= RC_PROTO_FLAGS_TX_STATS;
+		else
+			flags &= ~RC_PROTO_FLAGS_TX_STATS;
+	}
+	else if (!strcmp_P(res->cmd, PSTR("tx_power_probe"))) {
+		if (on == 1)
+			flags |= RC_PROTO_FLAGS_TX_POW_PROBE;
+		else
+			flags &= ~RC_PROTO_FLAGS_TX_POW_PROBE;
+	}
+	else if (!strcmp_P(res->cmd, PSTR("compute_best_pow"))) {
+		if (on == 1)
+			flags |= RC_PROTO_FLAGS_COMPUTE_BEST_POW;
+		else
+			flags &= ~RC_PROTO_FLAGS_COMPUTE_BEST_POW;
+	}
+	else if (!strcmp_P(res->cmd, PSTR("tx"))) {
+		flags &= ~RC_PROTO_FLAGS_TX_MASK;
+		if (!strcmp_P(res->val, PSTR("bypass")))
+			flags |= RC_PROTO_FLAGS_TX_BYPASS;
+		else if (!strcmp_P(res->val, PSTR("copy_spi")))
+			flags |= RC_PROTO_FLAGS_TX_COPY_SPI;
+	}
+	rc_proto_set_mode(flags);
+
+	/* dump state */
+	if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
+		printf_P(PSTR("rc_proto_mode tx off\n"));
+	else if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_BYPASS)
+		printf_P(PSTR("rc_proto_mode tx bypass\n"));
+	else if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_COPY_SPI)
+		printf_P(PSTR("rc_proto_mode tx copy_spi\n"));
+	printf_P(PSTR("rc_proto_mode rx_copy_spi %s\n"),
+		(flags & RC_PROTO_FLAGS_RX_COPY_SPI) ? "on" : "off");
+	printf_P(PSTR("rc_proto_mode rx_autobypass %s\n"),
+		(flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) ? "on" : "off");
+	printf_P(PSTR("rc_proto_mode tx_stats %s\n"),
+		(flags & RC_PROTO_FLAGS_TX_STATS) ? "on" : "off");
+	printf_P(PSTR("rc_proto_mode tx_power_probe %s\n"),
+		(flags & RC_PROTO_FLAGS_TX_POW_PROBE) ? "on" : "off");
+	printf_P(PSTR("rc_proto_mode compute_best_pow %s\n"),
+		(flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) ? "on" : "off");
+}
+
+const char PROGMEM str_rc_proto_mode_arg0[] = "rc_proto_mode";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_arg0 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, arg0,
+				 str_rc_proto_mode_arg0);
+
+const char PROGMEM str_rc_proto_mode_cmd[] =
+	"rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#compute_best_pow";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+		str_rc_proto_mode_cmd);
+
+const char PROGMEM str_rc_proto_mode_onoff[] = "on#off";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_onoff =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, val,
+				 str_rc_proto_mode_onoff);
+
+const char PROGMEM help_rc_proto_mode[] = "Set rc proto behavior";
+const parse_inst_t PROGMEM cmd_rc_proto_mode = {
+	.f = cmd_rc_proto_mode_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_rc_proto_mode,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_rc_proto_mode_arg0,
+		(PGM_P)&cmd_rc_proto_mode_cmd,
+		(PGM_P)&cmd_rc_proto_mode_onoff,
+		NULL,
+	},
+};
+
+const char PROGMEM str_rc_proto_mode_cmd2[] = "tx";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd2 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+		str_rc_proto_mode_cmd2);
+
+const char PROGMEM str_rc_proto_mode_val[] = "off#bypass#copy_spi";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_val =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, val,
+				 str_rc_proto_mode_val);
+
+const parse_inst_t PROGMEM cmd_rc_proto_mode2 = {
+	.f = cmd_rc_proto_mode_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_rc_proto_mode,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_rc_proto_mode_arg0,
+		(PGM_P)&cmd_rc_proto_mode_cmd2,
+		(PGM_P)&cmd_rc_proto_mode_val,
+		NULL,
+	},
+};
+
+const char PROGMEM str_rc_proto_mode_cmd3[] = "show";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd3 =
+	TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+		str_rc_proto_mode_cmd3);
+
+const parse_inst_t PROGMEM cmd_rc_proto_mode3 = {
+	.f = cmd_rc_proto_mode_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_rc_proto_mode,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_rc_proto_mode_arg0,
+		(PGM_P)&cmd_rc_proto_mode_cmd3,
+		NULL,
+	},
+};
+
+/**********************************************************/
+
 /* this structure is filled when cmd_rc_proto_hello is parsed successfully */
 struct cmd_rc_proto_hello_result {
 	fixed_string_t rc_proto_hello;
@@ -1916,7 +2150,6 @@ static void cmd_eeprom_add_parsed(void *parsed_result,
 	rdline_init(&rdl, cmdline_write_char, NULL, NULL);
 	rdline_newline(&rdl, "> ");
 
-	/* XXX bad: we should not block as we do not serve callout */
 	while (1) {
 		c = cmdline_dev_recv(NULL);
 		if (c < 0)
@@ -2013,6 +2246,54 @@ const parse_inst_t PROGMEM cmd_eeprom_list = {
 };
 
 
+/* ************* */
+
+struct cmd_dump_i2c_result {
+	fixed_string_t cmd;
+};
+
+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;
+
+	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\n"));
+		i2c_protocol_debug();
+		wait_ms(100);
+	}
+}
+
+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_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_dump_i2c,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_dump_i2c_cmd,
+		NULL,
+	},
+};
+
+
 /* ************* */
 
 /* in progmem */
@@ -2056,6 +2337,11 @@ const parse_ctx_t PROGMEM main_ctx[] = {
 	&cmd_test_spi,
 	&cmd_dump_xbee_stats,
 	&cmd_rc_proto_stats,
+	&cmd_rc_proto_timers,
+	&cmd_rc_proto_timers_show,
+	&cmd_rc_proto_mode,
+	&cmd_rc_proto_mode2,
+	&cmd_rc_proto_mode3,
 	&cmd_rc_proto_hello,
 	&cmd_rc_proto_hello_name,
 	&cmd_rc_proto_echo,
@@ -2065,5 +2351,6 @@ const parse_ctx_t PROGMEM main_ctx[] = {
 	&cmd_eeprom_add,
 	&cmd_eeprom_add2,
 	&cmd_eeprom_list,
+	&cmd_dump_i2c,
 	NULL,
 };