spi_servo.c

   1 #include <stdint.h>
   2 #include <stdio.h>
   3
   4 #include <aversive.h>
   5 #include <aversive/wait.h>
   6
   7 #include <scheduler.h>
   8
   9 #include "spi_servo.h"
  10 #include "main.h"
  11
  12 /*
  13  * The goal of this code is to send the servo commands to the slave
  14  * through the SPI bus. As the slave runs in polling mode to be
  15  * precise when generating servo signals, we cannot send data very
  16  * fast. We send one byte every ms, this is enough as we have at most
  17  * 6 servos (2 bytes) to update every 20ms
  18  *
  19  * When a new servo value is received, we send the first byte to the
  20  * SPI bus and store the next one. It will be transmitted by a
  21  * callback 1ms later. If new servos values are received during this
  22  * time, they are just saved but not transmitted until the first
  23  * command is issued. Once all commands have been transmitted, the
  24  * callback is unloaded.
  25  */
  26
  27 /* 1ms */
  28 #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
  29
  30 #define N_SERVO 6
  31 #define BYPASS_ENABLE 14
  32 #define BYPASS_DISABLE 15
  33 struct spi_servo_tx {
  34         uint16_t servo[N_SERVO];
  35         uint16_t cmd_mask;
  36         uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
  37         uint8_t cur_idx;
  38 };
  39 static struct spi_servo_tx spi_servo_tx;
  40
  41 struct spi_servo_rx {
  42         uint16_t servo[N_SERVO];
  43         uint8_t prev_byte;
  44 };
  45 static struct spi_servo_rx spi_servo_rx;
  46
  47 /*
  48  * SPI protocol:
  49  *
  50  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  51  * second one to 1. The first received byte contains the command number, and the
  52  * msb of the servo value. The second byte contains the lsb of the servo value.
  53  *
  54  * Commands 0 to NB_SERVO are to set the value of servo.
  55  * Command 14 is to enable bypass mode.
  56  * Command 15 is to disable bypass mode.
  57  */
  58 union spi_byte0 {
  59         uint8_t u8;
  60         struct {
  61                 /* inverted: little endian */
  62                 uint8_t val_msb:3;
  63                 uint8_t cmd_num:4;
  64                 uint8_t zero:1;
  65         };
  66 };
  67
  68 union spi_byte1 {
  69         uint8_t u8;
  70         struct {
  71                 /* inverted: little endian */
  72                 uint8_t val_lsb:7;
  73                 uint8_t one:1;
  74         };
  75 };
  76
  77 #define SS_HIGH() PORTB |= (1 << 4)
  78 #define SS_LOW() PORTB &= (~(1 << 4))
  79
  80 static void spi_send_byte(uint8_t byte)
  81 {
  82         SS_LOW();
  83         SPDR = byte;
  84         /* Wait for transmission complete (active loop is fine because
  85          * the clock is high) */
  86         while(!(SPSR & (1<<SPIF)));
  87         SS_HIGH();
  88 }
  89
  90 static void spi_send_one_servo(uint8_t num, uint16_t val)
  91 {
  92         union spi_byte0 byte0;
  93         union spi_byte1 byte1;
  94
  95         byte0.val_msb = val >> 7;
  96         if (num < N_SERVO)
  97                 byte0.cmd_num = num + 1;
  98         else
  99                 byte0.cmd_num = num;
 100         byte0.zero = 0;
 101         byte1.one = 1;
 102         byte1.val_lsb = val;
 103
 104         /* save the second byte */
 105         spi_servo_tx.next_byte = byte1.u8;
 106
 107         /* send the first byte */
 108         spi_send_byte(byte0.u8);
 109 }
 110
 111 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
 112 {
 113         uint8_t num;
 114         uint16_t val;
 115
 116         num = byte0.cmd_num - 1;
 117         if (num >= N_SERVO)
 118                 return;
 119
 120         val = byte0.val_msb;
 121         val <<= 7;
 122         val |= byte1.val_lsb;
 123
 124         spi_servo_rx.servo[num] = val;
 125 }
 126
 127 /* called by the scheduler */
 128 static void spi_servo_cb(void *arg)
 129 {
 130         uint8_t idx;
 131         union spi_byte0 byte0;
 132         union spi_byte1 byte1;
 133
 134         (void)arg;
 135
 136         /* get the value from the slave */
 137         byte0.u8 = SPDR;
 138         byte1.u8 = byte0.u8;
 139         if (byte0.zero == 0) {
 140                 spi_servo_rx.prev_byte = byte0.u8;
 141         }
 142         else {
 143                 byte0.u8 = spi_servo_rx.prev_byte;
 144                 decode_rx_servo(byte0, byte1);
 145         }
 146
 147         /* if next byte is set, send it */
 148         if (spi_servo_tx.next_byte != 0) {
 149                 spi_send_byte(spi_servo_tx.next_byte);
 150                 spi_servo_tx.next_byte = 0;
 151                 return;
 152         }
 153
 154         /* if there is no updated servo, send 0 and return. */
 155         if (spi_servo_tx.cmd_mask == 0) {
 156                 spi_send_byte(0);
 157                 return;
 158         }
 159
 160         /* else find it and send it */
 161         idx = spi_servo_tx.cur_idx;
 162         while (1) {
 163                 idx++;
 164                 if (idx == N_SERVO)
 165                         idx = BYPASS_ENABLE;
 166                 else if (idx >= sizeof(uint16_t) * 8)
 167                         idx = 0;
 168
 169                 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
 170                         break;
 171         }
 172
 173         spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
 174         spi_servo_tx.cmd_mask &= (~(1 << idx));
 175         spi_servo_tx.cur_idx = idx;
 176 }
 177
 178 void spi_servo_init(void)
 179 {
 180         /* SCK, SS & MOSI */
 181         DDRB = 0xb0;
 182
 183         /* remove power reduction on spi */
 184         PRR0 &= ~(1 << PRSPI);
 185
 186         /* Enable SPI, Master, set clock rate fck/64 */
 187         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
 188
 189         SS_HIGH();
 190
 191         scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
 192                                             SPI_EVT_PERIOD, SPI_PRIO);
 193         spi_servo_set(BYPASS_DISABLE, 0);
 194 }
 195
 196 void spi_servo_set(uint8_t num, uint16_t val)
 197 {
 198         uint8_t flags;
 199
 200         if (num >= N_SERVO)
 201                 return;
 202
 203         IRQ_LOCK(flags);
 204         spi_servo_tx.servo[num] = val;
 205         spi_servo_tx.cmd_mask |= (1 << num);
 206         IRQ_UNLOCK(flags);
 207 }
 208
 209 uint16_t spi_servo_get(uint8_t num)
 210 {
 211         uint8_t flags;
 212         uint16_t val;
 213
 214         if (num >= N_SERVO)
 215                 return 0;
 216
 217         IRQ_LOCK(flags);
 218         val = spi_servo_rx.servo[num];
 219         IRQ_UNLOCK(flags);
 220
 221         return val;
 222 }
 223
 224 void spi_servo_bypass(uint8_t enable)
 225 {
 226         uint8_t flags;
 227
 228         if (enable) {
 229                 IRQ_LOCK(flags);
 230                 spi_servo_tx.cmd_mask |= (1 << BYPASS_ENABLE);
 231                 IRQ_UNLOCK(flags);
 232         }
 233         else {
 234                 IRQ_LOCK(flags);
 235                 spi_servo_tx.cmd_mask |= (1 << BYPASS_DISABLE);
 236                 IRQ_UNLOCK(flags);
 237         }
 238 }