X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=commands.c;h=b58594ad58aae930a449ba07768159207f4afaeb;hp=fb67438cd64886b3ddb5dbd48f36acef76c0dc53;hb=9a43add2f0ed382ce1f180bba65fdc077d03b6fb;hpb=57895b3bb2fe0582c589685b7df34f3968b346ec diff --git a/commands.c b/commands.c index fb67438..b58594a 100644 --- a/commands.c +++ b/commands.c @@ -51,6 +51,7 @@ #include "rc_proto.h" #include "main.h" #include "cmdline.h" +#include "beep.h" /* commands_gen.c */ extern const parse_inst_t PROGMEM cmd_reset; @@ -1260,7 +1261,6 @@ const parse_inst_t PROGMEM cmd_debug = { }, }; -#ifndef USE_USB /**********************************************************/ /* this structure is filled when cmd_baudrate is parsed successfully */ @@ -1299,7 +1299,150 @@ const parse_inst_t PROGMEM cmd_baudrate = { NULL, }, }; -#endif + + +/**********************************************************/ + +/* this structure is filled when cmd_beep is parsed successfully */ +struct cmd_beep_result { + fixed_string_t beep; +}; + +/* function called when cmd_beep is parsed successfully */ +static void cmd_beep_parsed(void *parsed_result, void *data) +{ + (void)parsed_result; + (void)data; + + beep(0, 3, 3); + beep(1, 3, 3); + beep(2, 3, 3); + beep(0, 1, 1); + beep(1, 1, 1); + beep(2, 1, 1); +} + +const char PROGMEM str_beep[] = "beep"; +const parse_token_string_t PROGMEM cmd_beep_beep = + TOKEN_STRING_INITIALIZER(struct cmd_beep_result, beep, + str_beep); + +const char PROGMEM help_beep[] = "Send a beep"; + +const parse_inst_t PROGMEM cmd_beep = { + .f = cmd_beep_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beep, + .tokens = { /* token list, NULL terminated */ + (PGM_P)&cmd_beep_beep, + NULL, + }, +}; + +/**********************************************************/ + +/* this structure is filled when cmd_servo is parsed successfully */ +struct cmd_servo_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t num; + uint16_t val; +}; + +/* function called when cmd_servo is parsed successfully */ +static void cmd_servo_parsed(void * parsed_result, void *data) +{ + struct cmd_servo_result *res = parsed_result; + + (void)data; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + if (res->num >= N_SERVO) { + printf_P(PSTR("bad servo num\n")); + return; + } + if (res->val >= 1024) { + printf_P(PSTR("bad servo val\n")); + return; + } + spi_servo_set(res->num, res->val); + } + else if (!strcmp_P(res->arg1, PSTR("bypass"))) { + spi_servo_set_bypass(!!res->val); + } + else if (!strcmp_P(res->arg1, PSTR("ppm"))) { + spi_servo_set_ppm(!!res->val); + } + else if (!strcmp_P(res->arg1, PSTR("show"))) { + spi_servo_dump(); + } +} + +const char PROGMEM str_servo_arg0[] = "servo"; +const parse_token_string_t PROGMEM cmd_servo_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0, + str_servo_arg0); +const char PROGMEM str_servo_arg1_set[] = "set"; +const parse_token_string_t PROGMEM cmd_servo_arg1_set = + TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1, + str_servo_arg1_set); +const parse_token_num_t PROGMEM cmd_servo_num = + TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num, + UINT16); +const parse_token_num_t PROGMEM cmd_servo_val = + TOKEN_NUM_INITIALIZER(struct cmd_servo_result, val, + UINT16); + +const char PROGMEM help_servo_set[] = "set servo value"; +const parse_inst_t PROGMEM cmd_servo_set = { + .f = cmd_servo_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_set, + .tokens = { /* token list, NULL terminated */ + (PGM_P)&cmd_servo_arg0, + (PGM_P)&cmd_servo_arg1_set, + (PGM_P)&cmd_servo_num, + (PGM_P)&cmd_servo_val, + NULL, + }, +}; + +const char PROGMEM str_servo_arg1_show[] = "show"; +const parse_token_string_t PROGMEM cmd_servo_arg1_show = + TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1, + str_servo_arg1_show); + +const char PROGMEM help_servo_show[] = "read servo and config"; +const parse_inst_t PROGMEM cmd_servo_show = { + .f = cmd_servo_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_show, + .tokens = { /* token list, NULL terminated */ + (PGM_P)&cmd_servo_arg0, + (PGM_P)&cmd_servo_arg1_show, + NULL, + }, +}; + +const char PROGMEM str_servo_arg1_bypassppm[] = "bypass#ppm"; +const parse_token_string_t PROGMEM cmd_servo_arg1_bypassppm = + TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1, + str_servo_arg1_bypassppm); + +const char PROGMEM help_servo_bypassppm[] = "change bypass/ppm"; +const parse_inst_t PROGMEM cmd_servo_bypassppm = { + .f = cmd_servo_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_bypassppm, + .tokens = { /* token list, NULL terminated */ + (PGM_P)&cmd_servo_arg0, + (PGM_P)&cmd_servo_arg1_bypassppm, + (PGM_P)&cmd_servo_val, + NULL, + }, +}; + +/**********************************************************/ /* this structure is filled when cmd_test_spi is parsed successfully */ struct cmd_test_spi_result { @@ -1308,23 +1451,86 @@ struct cmd_test_spi_result { static void cmd_test_spi_parsed(void * parsed_result, void *data) { - int i; + uint8_t i, flags, wait_time = 0; + uint16_t val = 0; (void)parsed_result; (void)data; - while (1) { - for (i = 0; i < 50; i++) { - spi_servo_set(0, 0); - wait_ms(100); - spi_servo_set(0, 500); - wait_ms(100); + spi_servo_set_bypass(0); + spi_servo_set_ppm(0); + + /* stress test: send many commands, no wait between each servo + * of a series, and a variable delay between series */ + printf_P(PSTR("stress test\r\n")); + while (!cmdline_keypressed()) { + + wait_time++; + if (wait_time > 20) + wait_time = 0; + + IRQ_LOCK(flags); + val = global_ms; + IRQ_UNLOCK(flags); + val >>= 3; + val &= 1023; + + for (i = 0; i < 6; i++) + spi_servo_set(i, val); + + wait_ms(wait_time); + printf_P(PSTR("%4.4d %4.4d %4.4d %4.4d %4.4d %4.4d\r\n"), + spi_servo_get(0), spi_servo_get(1), spi_servo_get(2), + spi_servo_get(3), spi_servo_get(4), spi_servo_get(5)); + } + + printf_P(PSTR("bypass mode, with spi commands in background\r\n")); + spi_servo_set_bypass(1); + + /* test bypass mode */ + while (!cmdline_keypressed()) { + + wait_time++; + if (wait_time > 20) + wait_time = 0; + + IRQ_LOCK(flags); + val = global_ms; + IRQ_UNLOCK(flags); + val >>= 3; + val &= 1023; + + for (i = 0; i < 6; i++) + spi_servo_set(i, val); + + wait_ms(wait_time); + printf_P(PSTR("%4.4d %4.4d %4.4d %4.4d %4.4d %4.4d\r\n"), + spi_servo_get(0), spi_servo_get(1), spi_servo_get(2), + spi_servo_get(3), spi_servo_get(4), spi_servo_get(5)); + } + + printf_P(PSTR("PPM to servo\r\n")); + spi_servo_set_bypass(0); + spi_servo_set_ppm(0); + + /* test PPM to servo (bypass) mode */ + while (!cmdline_keypressed()) { + for (i = 0; i < 6; i++) { + val = spi_servo_get(i); + spi_servo_set(i, val); } + } - spi_servo_bypass(1); - wait_ms(10000); - spi_servo_bypass(0); - wait_ms(1); + printf_P(PSTR("PPM to (servo + PPM)\r\n")); + spi_servo_set_bypass(0); + spi_servo_set_ppm(1); + + /* test PPM to servo (bypass) mode */ + while (!cmdline_keypressed()) { + for (i = 0; i < 6; i++) { + val = spi_servo_get(i); + spi_servo_set(i, val); + } } } @@ -1379,9 +1585,11 @@ const parse_ctx_t PROGMEM main_ctx[] = { &cmd_raw, &cmd_dump, &cmd_debug, -#ifndef USE_USB &cmd_baudrate, -#endif + &cmd_beep, + &cmd_servo_set, + &cmd_servo_bypassppm, + &cmd_servo_show, &cmd_test_spi, NULL, };