-#include <aversive.h>
-#include <aversive/wait.h>
#include <stdint.h>
#include <stdio.h>
+#include <aversive.h>
+#include <aversive/wait.h>
+
+#include <scheduler.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.
+ */
-#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:
* 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 */
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<<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;
+ 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 */
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<<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;
+}
+
+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());
}
git.droids-corp.org / protos / xbee-avr.git / blobdiff