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