spi_servo.c

   1 #include <stdint.h>
   2 #include <stdio.h>
   3
   4 #include <aversive.h>
   5 #include <aversive/wait.h>
   6
   7 #include <callout.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 #define PPM_BIT    0x01
  28 #define BYPASS_BIT 0x02
  29
  30 struct spi_servo_tx {
  31         uint16_t servo[N_SERVO+1]; /* one more for control channel */
  32         uint16_t cmd_mask;
  33         uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
  34         uint8_t cur_idx;
  35 };
  36 static struct spi_servo_tx spi_servo_tx;
  37
  38 struct spi_servo_rx {
  39         uint16_t servo[N_SERVO];
  40         uint8_t prev_byte;
  41 };
  42 static struct spi_servo_rx spi_servo_rx;
  43
  44 /*
  45  * SPI protocol:
  46  *
  47  * A command is stored on 2 bytes. The first one has its msb to 0, and the
  48  * second one to 1. The first received byte contains the command number, and the
  49  * msb of the servo value. The second byte contains the lsb of the servo value.
  50  *
  51  * Command 0 is only one byte long, it means "I have nothing to say".
  52  * Commands 1 to N_SERVO (included) are to set the value of servo.
  53  * Command N_SERVO+1 is:
  54  * - to enable/disable ppm generation in place of last servo.
  55  * - to enable/disable bypass mode
  56  */
  57 union spi_byte0 {
  58         uint8_t u8;
  59         struct {
  60                 /* inverted: little endian */
  61                 uint8_t val_msb:3;
  62                 uint8_t cmd_num:4;
  63                 uint8_t zero:1;
  64         };
  65 };
  66
  67 union spi_byte1 {
  68         uint8_t u8;
  69         struct {
  70                 /* inverted: little endian */
  71                 uint8_t val_lsb:7;
  72                 uint8_t one:1;
  73         };
  74 };
  75
  76 #define SS_HIGH() PORTB |= (1 << 4)
  77 #define SS_LOW() PORTB &= (~(1 << 4))
  78
  79 static void spi_send_byte(uint8_t byte)
  80 {
  81         SS_LOW();
  82         SPDR = byte;
  83         /* Wait for transmission complete (active loop is fine because
  84          * the clock is high) */
  85         while(!(SPSR & (1<<SPIF)));
  86         SS_HIGH();
  87 }
  88
  89 static void spi_send_one_servo(uint8_t num, uint16_t val)
  90 {
  91         union spi_byte0 byte0;
  92         union spi_byte1 byte1;
  93
  94         byte0.val_msb = val >> 7;
  95         byte0.cmd_num = num + 1;
  96         byte0.zero = 0;
  97         byte1.one = 1;
  98         byte1.val_lsb = val;
  99
 100         /* save the second byte */
 101         spi_servo_tx.next_byte = byte1.u8;
 102
 103         /* send the first byte */
 104         spi_send_byte(byte0.u8);
 105 }
 106
 107 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
 108 {
 109         uint8_t num;
 110         uint16_t val;
 111
 112         num = byte0.cmd_num - 1;
 113         if (num >= N_SERVO)
 114                 return;
 115
 116         val = byte0.val_msb;
 117         val <<= 7;
 118         val |= byte1.val_lsb;
 119
 120         spi_servo_rx.servo[num] = val;
 121 }
 122
 123 /* called by the scheduler */
 124 static void spi_servo_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 125 {
 126         uint8_t idx;
 127         union spi_byte0 byte0;
 128         union spi_byte1 byte1;
 129
 130         (void)arg;
 131
 132         /* get the value from the slave */
 133         byte0.u8 = SPDR;
 134         byte1.u8 = byte0.u8;
 135         if (byte0.zero == 0) {
 136                 spi_servo_rx.prev_byte = byte0.u8;
 137         }
 138         else {
 139                 byte0.u8 = spi_servo_rx.prev_byte;
 140                 decode_rx_servo(byte0, byte1);
 141         }
 142
 143         /* if next byte is set, send it */
 144         if (spi_servo_tx.next_byte != 0) {
 145                 spi_send_byte(spi_servo_tx.next_byte);
 146                 spi_servo_tx.next_byte = 0;
 147                 goto reschedule;
 148         }
 149
 150         /* if there is no updated servo, send 0 and return. */
 151         if (spi_servo_tx.cmd_mask == 0) {
 152                 spi_send_byte(0);
 153                 goto reschedule;
 154         }
 155
 156         /* else find it and send it */
 157         idx = spi_servo_tx.cur_idx;
 158         while (1) {
 159                 idx++;
 160                 if (idx > N_SERVO + 1)
 161                         idx = 0;
 162
 163                 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
 164                         break;
 165         }
 166
 167         spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
 168         spi_servo_tx.cmd_mask &= (~(1 << idx));
 169         spi_servo_tx.cur_idx = idx;
 170
 171  reschedule:
 172         /* don't use callout_reschedule() here, we want to schedule in one tick
 173          * relative to current time: 1 tick is 682us at 12Mhz */
 174         callout_schedule(cm, tim, 0);
 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         callout_init(&xbeeboard.spi_timer, spi_servo_cb, NULL, SPI_PRIO);
 191         callout_schedule(&xbeeboard.intr_cm,
 192                 &xbeeboard.spi_timer, 0); /* immediate */
 193         spi_servo_set_bypass(1);
 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 uint8_t spi_servo_get_bypass(void)
 225 {
 226         return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
 227 }
 228
 229 uint8_t spi_servo_get_ppm(void)
 230 {
 231         return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
 232 }
 233
 234 void spi_servo_set_bypass(uint8_t enable)
 235 {
 236         uint8_t flags;
 237
 238         IRQ_LOCK(flags);
 239         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 240         if (enable)
 241                 spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
 242         else
 243                 spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
 244         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 245         IRQ_UNLOCK(flags);
 246 }
 247
 248 void spi_servo_set_ppm(uint8_t enable)
 249 {
 250         uint8_t flags;
 251
 252         IRQ_LOCK(flags);
 253         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 254         if (enable)
 255                 spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
 256         else
 257                 spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
 258         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
 259         IRQ_UNLOCK(flags);
 260 }
 261
 262 void spi_servo_dump(void)
 263 {
 264         uint8_t i;
 265
 266         for (i = 0; i < N_SERVO; i++)
 267                 printf_P(PSTR("%d: rx=%4.4d tx=%4.4d\r\n"), i,
 268                          spi_servo_get(i), spi_servo_tx.servo[i]);
 269         printf_P(PSTR("bypass=%d ppm=%d\n"),
 270                  spi_servo_get_bypass(), spi_servo_get_ppm());
 271 }