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