support servo control through SPI

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);
+               }

        }
 }
 
        }
 }
 

diff --git a/main.c b/main.c
index 2d1dd15..6d1710f 100644 (file)
--- a/main.c
+++ b/main.c
@@ -52,16 +52,6 @@
 #include <rdline.h>
 #include <timer.h>
 
 #include <rdline.h>
 #include <timer.h>
 

-#include "xbee_neighbor.h"
-#include "xbee_atcmd.h"
-#include "xbee_stats.h"
-#include "xbee_buf.h"
-#include "xbee_proto.h"
-#include "xbee.h"
-#include "cmdline.h"
-#include "callout.h"
-#include "rc_proto.h"
-#include "spi_servo.h"

 #include "main.h"
 
 struct xbeeboard xbeeboard;
 #include "main.h"
 
 struct xbeeboard xbeeboard;

diff --git a/main.h b/main.h
index 617628e..87070a3 100644 (file)
--- a/main.h
+++ b/main.h
@@ -25,6 +25,31 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 

+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/pgmspace.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <scheduler.h>
+#include <clock_time.h>
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+
+#include "xbee_neighbor.h"
+#include "xbee_atcmd.h"
+#include "xbee_stats.h"
+#include "xbee_buf.h"
+#include "xbee_proto.h"
+#include "xbee.h"
+#include "cmdline.h"
+#include "callout.h"
+#include "rc_proto.h"
+#include "spi_servo.h"
+
+extern volatile uint16_t global_ms;
+

 #define NB_LOGS 4
 
 /** ERROR NUMS */
 #define NB_LOGS 4
 
 /** ERROR NUMS */


@@ -39,8 +64,11 @@
 #define LED3_ON()       sbi(PORTA, 0)
 #define LED3_OFF()      cbi(PORTA, 0)
 
 #define LED3_ON()       sbi(PORTA, 0)
 #define LED3_OFF()      cbi(PORTA, 0)
 

+/* highest priority */

 #define LED_PRIO           170
 #define TIME_PRIO          160
 #define LED_PRIO           170
 #define TIME_PRIO          160

+#define SPI_PRIO           100 /* users of spi_servo must have lower prio */
+/* lowest priority */

 
 #define MAX_POWER_LEVEL 5
 /* generic to all boards */
 
 #define MAX_POWER_LEVEL 5
 /* generic to all boards */

diff --git a/spi_servo.c b/spi_servo.c
index 59a0810..69afed2 100644 (file)
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -1,12 +1,48 @@
-#include <aversive.h>
-#include <aversive/wait.h>

 #include <stdint.h>
 #include <stdio.h>
 
 #include <stdint.h>
 #include <stdio.h>
 

+#include <aversive.h>
+#include <aversive/wait.h>
+
+#include <scheduler.h>
+

 #include "spi_servo.h"
 #include "spi_servo.h"

+#include "main.h"
+
+/*
+ * The goal of this code is to send the servo commands to the slave
+ * through the SPI bus. As the slave runs in polling mode to be
+ * precise when generating servo signals, we cannot send data very
+ * fast. We send one byte every ms, this is enough as we have at most
+ * 6 servos (2 bytes) to update every 20ms
+ *
+ * When a new servo value is received, we send the first byte to the
+ * SPI bus and store the next one. It will be transmitted by a
+ * callback 1ms later. If new servos values are received during this
+ * time, they are just saved but not transmitted until the first
+ * command is issued. Once all commands have been transmitted, the
+ * callback is unloaded.
+ */

 
 

+/* 1ms */
+#define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
+
+#define N_SERVO 6

 #define BYPASS_ENABLE 14
 #define BYPASS_DISABLE 15
 #define BYPASS_ENABLE 14
 #define BYPASS_DISABLE 15

+struct spi_servo_tx {
+       uint16_t servo[N_SERVO];
+       uint16_t cmd_mask;
+       uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
+       uint8_t cur_idx;
+};
+static struct spi_servo_tx spi_servo_tx;
+
+struct spi_servo_rx {
+       uint16_t servo[N_SERVO];
+       uint8_t prev_byte;
+};
+static struct spi_servo_rx spi_servo_rx;

 
 /*
  * SPI protocol:
 
 /*
  * SPI protocol:


@@ -19,7 +55,7 @@
  * Command 14 is to enable bypass mode.
  * Command 15 is to disable bypass mode.
  */
  * Command 14 is to enable bypass mode.
  * Command 15 is to disable bypass mode.
  */

-static volatile union {
+union spi_byte0 {

        uint8_t u8;
        struct {
                /* inverted: little endian */
        uint8_t u8;
        struct {
                /* inverted: little endian */


@@ -27,20 +63,118 @@ static volatile union {
                uint8_t cmd_num:4;
                uint8_t zero:1;
        };
                uint8_t cmd_num:4;
                uint8_t zero:1;
        };

-} byte0;
+};

 
 

-static volatile union {
+union spi_byte1 {

        uint8_t u8;
        struct {
                /* inverted: little endian */
                uint8_t val_lsb:7;
                uint8_t one:1;
        };
        uint8_t u8;
        struct {
                /* inverted: little endian */
                uint8_t val_lsb:7;
                uint8_t one:1;
        };

-} byte1;
+};

 
 #define SS_HIGH() PORTB |= (1 << 4)
 #define SS_LOW() PORTB &= (~(1 << 4))
 
 
 #define SS_HIGH() PORTB |= (1 << 4)
 #define SS_LOW() PORTB &= (~(1 << 4))
 

+static void spi_send_byte(uint8_t byte)
+{
+       SS_LOW();
+       SPDR = byte;
+       /* Wait for transmission complete (active loop is fine because
+        * the clock is high) */
+       while(!(SPSR & (1<<SPIF)));
+       SS_HIGH();
+}
+
+static void spi_send_one_servo(uint8_t num, uint16_t val)
+{
+       union spi_byte0 byte0;
+       union spi_byte1 byte1;
+
+       byte0.val_msb = val >> 7;
+       if (num < N_SERVO)
+               byte0.cmd_num = num + 1;
+       else
+               byte0.cmd_num = num;
+       byte0.zero = 0;
+       byte1.one = 1;
+       byte1.val_lsb = val;
+
+       /* save the second byte */
+       spi_servo_tx.next_byte = byte1.u8;
+
+       /* send the first byte */
+       spi_send_byte(byte0.u8);
+}
+
+static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
+{
+       uint8_t num;
+       uint16_t val;
+
+       num = byte0.cmd_num - 1;
+       if (num >= N_SERVO)
+               return;
+
+       val = byte0.val_msb;
+       val <<= 7;
+       val |= byte1.val_lsb;
+
+       spi_servo_rx.servo[num] = val;
+}
+
+/* called by the scheduler */
+static void spi_servo_cb(void *arg)
+{
+       uint8_t idx;
+       union spi_byte0 byte0;
+       union spi_byte1 byte1;
+
+       (void)arg;
+
+       /* get the value from the slave */
+       byte0.u8 = SPDR;
+       byte1.u8 = byte0.u8;
+       if (byte0.zero == 0) {
+               spi_servo_rx.prev_byte = byte0.u8;
+       }
+       else {
+               byte0.u8 = spi_servo_rx.prev_byte;
+               decode_rx_servo(byte0, byte1);
+       }
+
+       /* if next byte is set, send it */
+       if (spi_servo_tx.next_byte != 0) {
+               spi_send_byte(spi_servo_tx.next_byte);
+               spi_servo_tx.next_byte = 0;
+               return;
+       }
+
+       /* if there is no updated servo, send 0 and return. */
+       if (spi_servo_tx.cmd_mask == 0) {
+               spi_send_byte(0);
+               return;
+       }
+
+       /* else find it and send it */
+       idx = spi_servo_tx.cur_idx;
+       while (1) {
+               idx++;
+               if (idx == N_SERVO)
+                       idx = BYPASS_ENABLE;
+               else if (idx >= sizeof(uint16_t) * 8)
+                       idx = 0;
+
+               if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
+                       break;
+       }
+
+       spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
+       spi_servo_tx.cmd_mask &= (~(1 << idx));
+       spi_servo_tx.cur_idx = idx;
+}
+

 void spi_servo_init(void)
 {
        /* SCK, SS & MOSI */
 void spi_servo_init(void)
 {
        /* SCK, SS & MOSI */


@@ -54,36 +188,51 @@ void spi_servo_init(void)
 
        SS_HIGH();
 
 
        SS_HIGH();
 

+       scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
+                                           SPI_EVT_PERIOD, SPI_PRIO);

        spi_servo_set(BYPASS_DISABLE, 0);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
 {
        spi_servo_set(BYPASS_DISABLE, 0);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
 {

-       byte0.val_msb = val >> 7;
-       byte0.cmd_num = num;
-       byte0.zero = 0;
-       byte1.one = 1;
-       byte1.val_lsb = val;
+       uint8_t flags;

 
 

-       SS_LOW();
-       SPDR = byte0.u8;
-       /* Wait for transmission complete */
-       while(!(SPSR & (1<<SPIF)));
-       SS_HIGH();
+       if (num >= N_SERVO)
+               return;

 
 

-       _delay_loop_1(5);
-       SS_LOW();
+       IRQ_LOCK(flags);
+       spi_servo_tx.servo[num] = val;
+       spi_servo_tx.cmd_mask |= (1 << num);
+       IRQ_UNLOCK(flags);
+}

 
 

-       SPDR = byte1.u8;
-       /* Wait for transmission complete */
-       while(!(SPSR & (1<<SPIF)));
-       SS_HIGH();
+uint16_t spi_servo_get(uint8_t num)
+{
+       uint8_t flags;
+       uint16_t val;
+
+       if (num >= N_SERVO)
+               return 0;
+
+       IRQ_LOCK(flags);
+       val = spi_servo_rx.servo[num];
+       IRQ_UNLOCK(flags);
+
+       return val;

 }
 
 void spi_servo_bypass(uint8_t enable)
 {
 }
 
 void spi_servo_bypass(uint8_t enable)
 {

-       if (enable)
-               spi_servo_set(BYPASS_ENABLE, 0);
-       else
-               spi_servo_set(BYPASS_DISABLE, 0);
+       uint8_t flags;
+
+       if (enable) {
+               IRQ_LOCK(flags);
+               spi_servo_tx.cmd_mask |= (1 << BYPASS_ENABLE);
+               IRQ_UNLOCK(flags);
+       }
+       else {
+               IRQ_LOCK(flags);
+               spi_servo_tx.cmd_mask |= (1 << BYPASS_DISABLE);
+               IRQ_UNLOCK(flags);
+       }

 }
 }

diff --git a/spi_servo.h b/spi_servo.h
index 188f8cd..cca0b76 100644 (file)
--- a/spi_servo.h
+++ b/spi_servo.h
@@ -1,3 +1,4 @@
 void spi_servo_init(void);
 void spi_servo_set(uint8_t num, uint16_t val);
 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_bypass(uint8_t enable);