X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=spi_servo.c;h=c180bc907c9933e954df5bcbb0b136c2d6398b12;hp=59a0810463748b10a232b47345bac15c572f5e31;hb=f74db5fd03bcd38af3ca533e7531dd70454dfaaf;hpb=57895b3bb2fe0582c589685b7df34f3968b346ec diff --git a/spi_servo.c b/spi_servo.c index 59a0810..c180bc9 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. + */ -#define BYPASS_ENABLE 14 -#define BYPASS_DISABLE 15 +/* 1 scheduler unit (682us at 12Mhz) */ +#define SPI_EVT_PERIOD (1) + +#define PPM_BIT 0x01 +#define BYPASS_BIT 0x02 + +struct spi_servo_tx { + uint16_t servo[N_SERVO+1]; /* one more for control channel */ + 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: @@ -15,11 +51,13 @@ * second one to 1. The first received byte contains the command number, and the * msb of the servo value. The second byte contains the lsb of the servo value. * - * Commands 0 to NB_SERVO are to set the value of servo. - * Command 14 is to enable bypass mode. - * Command 15 is to disable bypass mode. + * Command 0 is only one byte long, it means "I have nothing to say". + * Commands 1 to N_SERVO (included) are to set the value of servo. + * Command N_SERVO+1 is: + * - to enable/disable ppm generation in place of last servo. + * - to enable/disable bypass mode */ -static volatile union { +union spi_byte0 { uint8_t u8; struct { /* inverted: little endian */ @@ -27,20 +65,113 @@ 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; + byte0.cmd_num = num + 1; + 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 + 1) + 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 +185,84 @@ void spi_servo_init(void) SS_HIGH(); - spi_servo_set(BYPASS_DISABLE, 0); + scheduler_add_periodical_event_priority(&spi_servo_cb, NULL, + SPI_EVT_PERIOD, SPI_PRIO); + spi_servo_set_bypass(1); } 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; +} + +uint8_t spi_servo_get_bypass(void) +{ + return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT); } -void spi_servo_bypass(uint8_t enable) +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; + + IRQ_LOCK(flags); + spi_servo_tx.cmd_mask |= (1 << N_SERVO); if (enable) - spi_servo_set(BYPASS_ENABLE, 0); + spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT; else - spi_servo_set(BYPASS_DISABLE, 0); + spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT); + spi_servo_tx.cmd_mask |= (1 << N_SERVO); + IRQ_UNLOCK(flags); +} + +void spi_servo_set_ppm(uint8_t enable) +{ + uint8_t flags; + + IRQ_LOCK(flags); + spi_servo_tx.cmd_mask |= (1 << N_SERVO); + if (enable) + spi_servo_tx.servo[N_SERVO] |= PPM_BIT; + else + spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT); + spi_servo_tx.cmd_mask |= (1 << N_SERVO); + IRQ_UNLOCK(flags); +} + +void spi_servo_dump(void) +{ + uint8_t i; + + for (i = 0; i < N_SERVO; i++) + printf_P(PSTR("%d: rx=%4.4d tx=%4.4d\r\n"), i, + spi_servo_get(i), spi_servo_tx.servo[i]); + printf_P(PSTR("bypass=%d ppm=%d\n"), + spi_servo_get_bypass(), spi_servo_get_ppm()); }