X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=spi_servo.c;h=69afed24ba1baf9741acbd6a29ecf0d660db45e3;hp=59a0810463748b10a232b47345bac15c572f5e31;hb=2710a676cfb8d84c4e4de9d4e0ba2e8f3b553120;hpb=8fcd09b3ebe4880910ea909e298f2eeb563ac6fe 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); + } }