From 2710a676cfb8d84c4e4de9d4e0ba2e8f3b553120 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 3 Oct 2013 20:37:10 +0200 Subject: [PATCH] support servo control through SPI --- commands.c | 76 ++++++++++++++++---- main.c | 10 --- main.h | 28 ++++++++ spi_servo.c | 201 +++++++++++++++++++++++++++++++++++++++++++++------- spi_servo.h | 1 + 5 files changed, 268 insertions(+), 48 deletions(-) diff --git a/commands.c b/commands.c index dee0693..4389a6b 100644 --- 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) { - 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); + } } } diff --git a/main.c b/main.c index 2d1dd15..6d1710f 100644 --- a/main.c +++ b/main.c @@ -52,16 +52,6 @@ #include #include -#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; diff --git a/main.h b/main.h index 617628e..87070a3 100644 --- a/main.h +++ b/main.h @@ -25,6 +25,31 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 */ @@ -39,8 +64,11 @@ #define LED3_ON() sbi(PORTA, 0) #define LED3_OFF() cbi(PORTA, 0) +/* highest priority */ #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 */ diff --git a/spi_servo.c b/spi_servo.c index 59a0810..69afed2 100644 --- a/spi_servo.c +++ b/spi_servo.c @@ -1,12 +1,48 @@ -#include -#include #include #include +#include +#include + +#include + #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 +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: @@ -19,7 +55,7 @@ * 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 */ @@ -27,20 +63,118 @@ static volatile union { 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; }; -} byte1; +}; #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<> 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 */ @@ -54,36 +188,51 @@ void spi_servo_init(void) 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) { - 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<= 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<= 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) { - 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 --- 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); +uint16_t spi_servo_get(uint8_t num); void spi_servo_bypass(uint8_t enable); -- 2.39.5