support servo control through SPI

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

diff --git a/commands.c b/commands.c
index dee0693..4389a6b 100644 (file)
--- a/commands.c
+++ b/commands.c
@@ -1306,23 +1306,75 @@ struct cmd_test_spi_result {
 
 static void cmd_test_spi_parsed(void * parsed_result, void *data)
 {
 
 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;
 
 
        (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);
+               }

        }
 }
 
        }
 }