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
  32 #define PPM_BIT    0x01
  33 #define BYPASS_BIT 0x02
  34
  35 struct spi_servo_tx {
  36         uint16_t servo[N_SERVO+1]; /* one more for control channel */
  37         uint16_t cmd_mask;
  38         uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
  39         uint8_t cur_idx;
  40 };
  41 static struct spi_servo_tx spi_servo_tx;
  42
  43 struct spi_servo_rx {
  44         uint16_t servo[N_SERVO];
  45         uint8_t prev_byte;
  46 };
  47 static struct spi_servo_rx spi_servo_rx;
  48
  49 /*
  50  * SPI protocol:
  51  *
  52  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  53  * second one to 1. The first received byte contains the command number, and the
  54  * msb of the servo value. The second byte contains the lsb of the servo value.
  55  *
  56  * Command 0 is only one byte long, it means "I have nothing to say".
  57  * Commands 1 to N_SERVO (included) are to set the value of servo.
  58  * Command N_SERVO+1 is:
  59  * - to enable/disable ppm generation in place of last servo.
  60  * - to enable/disable bypass mode
  61  */
  62 union spi_byte0 {
  63         uint8_t u8;
  64         struct {
  65                 /* inverted: little endian */
  66                 uint8_t val_msb:3;
  67                 uint8_t cmd_num:4;
  68                 uint8_t zero:1;
  69         };
  70 };
  71
  72 union spi_byte1 {
  73         uint8_t u8;
  74         struct {
  75                 /* inverted: little endian */
  76                 uint8_t val_lsb:7;
  77                 uint8_t one:1;
  78         };
  79 };
  80
  81 #define SS_HIGH() PORTB |= (1 << 4)
  82 #define SS_LOW() PORTB &= (~(1 << 4))
  83
  84 static void spi_send_byte(uint8_t byte)
  85 {
  86         SS_LOW();
  87         SPDR = byte;
  88         /* Wait for transmission complete (active loop is fine because
  89          * the clock is high) */
  90         while(!(SPSR & (1<<SPIF)));
  91         SS_HIGH();
  92 }
  93
  94 static void spi_send_one_servo(uint8_t num, uint16_t val)
  95 {
  96         union spi_byte0 byte0;
  97         union spi_byte1 byte1;
  98
  99         byte0.val_msb = val >> 7;
 100         byte0.cmd_num = num + 1;
 101         byte0.zero = 0;
 102         byte1.one = 1;
 103         byte1.val_lsb = val;
 104
 105         /* save the second byte */
 106         spi_servo_tx.next_byte = byte1.u8;
 107
 108         /* send the first byte */
 109         spi_send_byte(byte0.u8);
 110 }
 111
 112 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
 113 {
 114         uint8_t num;
 115         uint16_t val;
 116
 117         num = byte0.cmd_num - 1;
 118         if (num >= N_SERVO)
 119                 return;
 120
 121         val = byte0.val_msb;
 122         val <<= 7;
 123         val |= byte1.val_lsb;
 124
 125         spi_servo_rx.servo[num] = val;
 126 }
 127
 128 /* called by the scheduler */
 129 static void spi_servo_cb(void *arg)
 130 {
 131         uint8_t idx;
 132         union spi_byte0 byte0;
 133         union spi_byte1 byte1;
 134
 135         (void)arg;
 136
 137         /* get the value from the slave */
 138         byte0.u8 = SPDR;
 139         byte1.u8 = byte0.u8;
 140         if (byte0.zero == 0) {
 141                 spi_servo_rx.prev_byte = byte0.u8;
 142         }
 143         else {
 144                 byte0.u8 = spi_servo_rx.prev_byte;
 145                 decode_rx_servo(byte0, byte1);
 146         }
 147
 148         /* if next byte is set, send it */
 149         if (spi_servo_tx.next_byte != 0) {
 150                 spi_send_byte(spi_servo_tx.next_byte);
 151                 spi_servo_tx.next_byte = 0;
 152                 return;
 153         }
 154
 155         /* if there is no updated servo, send 0 and return. */
 156         if (spi_servo_tx.cmd_mask == 0) {
 157                 spi_send_byte(0);
 158                 return;
 159         }
 160
 161         /* else find it and send it */
 162         idx = spi_servo_tx.cur_idx;
 163         while (1) {
 164                 idx++;
 165                 if (idx > N_SERVO + 1)
 166                         idx = 0;
 167
 168                 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
 169                         break;
 170         }
 171
 172         spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
 173         spi_servo_tx.cmd_mask &= (~(1 << idx));
 174         spi_servo_tx.cur_idx = idx;
 175 }
 176
 177 void spi_servo_init(void)
 178 {
 179         /* SCK, SS & MOSI */
 180         DDRB = 0xb0;
 181
 182         /* remove power reduction on spi */
 183         PRR0 &= ~(1 << PRSPI);
 184
 185         /* Enable SPI, Master, set clock rate fck/64 */
 186         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
 187
 188         SS_HIGH();
 189
 190         scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
 191                                             SPI_EVT_PERIOD, SPI_PRIO);
 192         spi_servo_bypass(1);
 193 }
 194
 195 void spi_servo_set(uint8_t num, uint16_t val)
 196 {
 197         uint8_t flags;
 198
 199         if (num >= N_SERVO)
 200                 return;
 201
 202         IRQ_LOCK(flags);
 203         spi_servo_tx.servo[num] = val;
 204         spi_servo_tx.cmd_mask |= (1 << num);
 205         IRQ_UNLOCK(flags);
 206 }
 207
 208 uint16_t spi_servo_get(uint8_t num)
 209 {
 210         uint8_t flags;
 211         uint16_t val;
 212
 213         if (num >= N_SERVO)
 214                 return 0;
 215
 216         IRQ_LOCK(flags);
 217         val = spi_servo_rx.servo[num];
 218         IRQ_UNLOCK(flags);
 219
 220         return val;
 221 }
 222
 223 void spi_servo_bypass(uint8_t enable)
 224 {
 225         uint8_t flags;
 226
 227         IRQ_LOCK(flags);
 228         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 229         if (enable)
 230                 spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
 231         else
 232                 spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
 233         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 234         IRQ_UNLOCK(flags);
 235 }
 236
 237 void spi_servo_ppm(uint8_t enable)
 238 {
 239         uint8_t flags;
 240
 241         IRQ_LOCK(flags);
 242         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 243         if (enable)
 244                 spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
 245         else
 246                 spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
 247         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 248         IRQ_UNLOCK(flags);
 249 }