support servo control through SPI

[protos/xbee-avr.git] / commands.c

diff --git a/commands.c b/commands.c
index fb67438..4389a6b 100644 (file)
--- a/commands.c
+++ b/commands.c
@@ -1260,7 +1260,6 @@ const parse_inst_t PROGMEM cmd_debug = {
        },
 };
 
-#ifndef USE_USB
 /**********************************************************/
 
 /* this structure is filled when cmd_baudrate is parsed successfully */
@@ -1299,7 +1298,6 @@ const parse_inst_t PROGMEM cmd_baudrate = {
                NULL,
        },
 };
-#endif
 
 /* this structure is filled when cmd_test_spi is parsed successfully */
 struct cmd_test_spi_result {
@@ -1308,23 +1306,75 @@ 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_bypass(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);
+
+               for (i = 0; i < 6; i++)
+                       printf_P(PSTR("%d: %d\r\n"), i, spi_servo_get(i));
+               printf_P(PSTR("\r\n"));
+       }
+
+       printf_P(PSTR("bypass mode, with spi commands in background\r\n"));
+       spi_servo_bypass(1);
+
+       /* test bypass mode */
+       while (!cmdline_keypressed()) {
 
-               spi_servo_bypass(1);
-               wait_ms(10000);
-               spi_servo_bypass(0);
-               wait_ms(1);
+               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);
+
+               for (i = 0; i < 6; i++)
+                       printf_P(PSTR("%d: %d\r\n"), i, spi_servo_get(i));
+               printf_P(PSTR("\r\n"));
+       }
+
+       spi_servo_bypass(0);
+
+       printf_P(PSTR("PPM to servo\r\n"));
+
+       /* 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 +1429,7 @@ const parse_ctx_t PROGMEM main_ctx[] = {
        &cmd_raw,
        &cmd_dump,
        &cmd_debug,
-#ifndef USE_USB
        &cmd_baudrate,
-#endif
        &cmd_test_spi,
        NULL,
 };