add a command to read/write servo

diff --git a/commands.c b/commands.c
index 083f85fa4fe17bb68f640a616ead8893fb7673b5..ce85088fcc30b4f1b80cd73e04169f14beb5f181 100644 (file)
--- a/commands.c
+++ b/commands.c
@@ -1299,6 +1299,116 @@ const parse_inst_t PROGMEM cmd_baudrate = {
        },
 };
 
+/**********************************************************/
+
+/* 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;
+       uint8_t i;
+
+       (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"))) {
+               for (i = 0; i < 6; i++)
+                       printf_P(PSTR("%d: %d\r\n"), i, spi_servo_get(i));
+               printf_P(PSTR("bypass=%d ppm=%d\n"),
+                        spi_servo_get_bypass(), spi_servo_get_ppm());
+               printf_P(PSTR("\r\n"));
+       }
+}
+
+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 {
        fixed_string_t arg0;
@@ -1312,13 +1422,13 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
        (void)parsed_result;
        (void)data;
 
-       spi_servo_bypass(0);
-       spi_servo_ppm(0);
+       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()) {
+       while (!cmdline_keypressed()) {
 
                wait_time++;
                if (wait_time > 20)
@@ -1341,7 +1451,7 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
        }
 
        printf_P(PSTR("bypass mode, with spi commands in background\r\n"));
-       spi_servo_bypass(1);
+       spi_servo_set_bypass(1);
 
        /* test bypass mode */
        while (!cmdline_keypressed()) {
@@ -1367,8 +1477,8 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
        }
 
        printf_P(PSTR("PPM to servo\r\n"));
-       spi_servo_bypass(0);
-       spi_servo_ppm(0);
+       spi_servo_set_bypass(0);
+       spi_servo_set_ppm(0);
 
        /* test PPM to servo (bypass) mode */
        while (!cmdline_keypressed()) {
@@ -1379,8 +1489,8 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
        }
 
        printf_P(PSTR("PPM to (servo + PPM)\r\n"));
-       spi_servo_bypass(0);
-       spi_servo_ppm(1);
+       spi_servo_set_bypass(0);
+       spi_servo_set_ppm(1);
 
        /* test PPM to servo (bypass) mode */
        while (!cmdline_keypressed()) {
@@ -1443,6 +1553,9 @@ const parse_ctx_t PROGMEM main_ctx[] = {
        &cmd_dump,
        &cmd_debug,
        &cmd_baudrate,
+       &cmd_servo_set,
+       &cmd_servo_bypassppm,
+       &cmd_servo_show,
        &cmd_test_spi,
        NULL,
 };

diff --git a/spi_servo.c b/spi_servo.c
index 6e1da056a731aba4ea0024715825263bb9ae4ea6..c4fe3187dcd02e3084e70b4ba50bbdae279c166c 100644 (file)
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -27,8 +27,6 @@
 /* 1ms */
 #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
 
-#define N_SERVO 6
-
 #define PPM_BIT    0x01
 #define BYPASS_BIT 0x02
 
@@ -189,7 +187,7 @@ void spi_servo_init(void)
 
        scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
                                            SPI_EVT_PERIOD, SPI_PRIO);
-       spi_servo_bypass(1);
+       spi_servo_set_bypass(1);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
@@ -220,7 +218,17 @@ uint16_t spi_servo_get(uint8_t num)
        return val;
 }
 
-void spi_servo_bypass(uint8_t enable)
+uint8_t spi_servo_get_bypass(void)
+{
+       return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
+}
+
+uint8_t spi_servo_get_ppm(void)
+{
+       return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
+}
+
+void spi_servo_set_bypass(uint8_t enable)
 {
        uint8_t flags;
 
@@ -234,7 +242,7 @@ void spi_servo_bypass(uint8_t enable)
        IRQ_UNLOCK(flags);
 }
 
-void spi_servo_ppm(uint8_t enable)
+void spi_servo_set_ppm(uint8_t enable)
 {
        uint8_t flags;
 

diff --git a/spi_servo.h b/spi_servo.h
index 7156166a57e3bdd4453938f990986565655abfa2..895df3a3374d090c02c24814493d15d439a9e234 100644 (file)
--- a/spi_servo.h
+++ b/spi_servo.h
@@ -1,5 +1,9 @@
+#define N_SERVO 6
+
 void spi_servo_init(void);
 void spi_servo_set(uint8_t num, uint16_t val);
 uint16_t spi_servo_get(uint8_t num);
-void spi_servo_bypass(uint8_t enable);
-void spi_servo_ppm(uint8_t enable);
+void spi_servo_set_bypass(uint8_t enable);
+void spi_servo_set_ppm(uint8_t enable);
+uint8_t spi_servo_get_bypass(void);
+uint8_t spi_servo_get_ppm(void);