X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=spi_servo.c;h=69afed24ba1baf9741acbd6a29ecf0d660db45e3;hb=2710a676cfb8d84c4e4de9d4e0ba2e8f3b553120;hp=fad6866ea25dda2407a712d1d8992a8ca7b6879f;hpb=9a9b64112aee5ab26398b46cb13b7e49c292a355;p=protos%2Fxbee-avr.git diff --git a/spi_servo.c b/spi_servo.c index fad6866..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,23 +63,122 @@ 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) { - /* real SS ! */ - DDRK = 0x2; /* SCK, SS & MOSI */ - DDRB = 0x7; + DDRB = 0xb0; /* remove power reduction on spi */ PRR0 &= ~(1 << PRSPI); @@ -51,38 +186,53 @@ void spi_servo_init(void) /* Enable SPI, Master, set clock rate fck/64 */ SPCR = (1<> 7; - byte0.cmd_num = num; - byte0.zero = 0; - byte1.one = 1; - byte1.val_lsb = val; + uint8_t flags; - PORTK &= ~(1<<1); - SPDR = byte0.u8; - /* Wait for transmission complete */ - while(!(SPSR & (1<= N_SERVO) + return; + + IRQ_LOCK(flags); + spi_servo_tx.servo[num] = val; + spi_servo_tx.cmd_mask |= (1 << num); + IRQ_UNLOCK(flags); +} + +uint16_t spi_servo_get(uint8_t num) +{ + uint8_t flags; + uint16_t val; - _delay_loop_1(5); - PORTK &= ~(1<<1); + if (num >= N_SERVO) + return 0; - SPDR = byte1.u8; - /* Wait for transmission complete */ - while(!(SPSR & (1<