main.c

   1 #include <aversive.h>
   2 #include <aversive/wait.h>
   3
   4 struct servo {
   5         uint8_t bit;
   6         uint16_t command;
   7 };
   8
   9 static struct servo servo_table[] = {
  10         {
  11                 .bit = 2,
  12                 .command = 300,
  13         },
  14         {
  15                 .bit = 3,
  16                 .command = 700,
  17         },
  18         {
  19                 .bit = 4,
  20                 .command = 512,
  21         },
  22         {
  23                 .bit = 5,
  24                 .command = 512,
  25         },
  26         {
  27                 .bit = 6,
  28                 .command = 512,
  29         },
  30         {
  31                 .bit = 7,
  32                 .command = 512,
  33         },
  34 };
  35 #define N_SERVO (sizeof(servo_table)/sizeof(*servo_table))
  36
  37 /* we use the first servo for PPM output if enabled */
  38 #define PPM (0)
  39
  40 static uint8_t bypass;
  41 static uint8_t ppm_enabled;
  42 static volatile uint8_t done;
  43 static uint8_t portval;
  44 static uint8_t rxidx;
  45
  46 static uint8_t icp_idx = N_SERVO;
  47 static uint16_t icp_servos[N_SERVO];
  48 static uint16_t icp_prev;
  49
  50 static uint8_t spi_out_idx; /* current byte beeing sent */
  51
  52 #define PPM_BIT    0x01
  53 #define BYPASS_BIT 0x02
  54
  55 #define LED_ON() do { PORTB |= 0x02; } while(0)
  56 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
  57
  58 /*
  59  * SPI protocol:
  60  *
  61  * A command is stored on 2 bytes (except command 0). The first byte
  62  * has its most significant bit to 0, and the second one to 1. The
  63  * first received byte contains the command number, and the msb of the
  64  * servo value. The second byte contains the lsb of the servo value.
  65  *
  66  * Command 0 is only one byte long, it means "I have nothing to say".
  67  * Commands 1 to N_SERVO (included) are to set the value of servo.
  68  * Command N_SERVO+1 is:
  69  * - to enable/disable ppm generation in place of first servo.
  70  * - to enable/disable bypass mode
  71  */
  72 union byte0 {
  73         uint8_t u8;
  74         struct {
  75                 /* inverted: little endian */
  76                 uint8_t val_msb:3;
  77                 uint8_t cmd_num:4;
  78                 uint8_t zero:1;
  79         };
  80 };
  81
  82 union byte1 {
  83         uint8_t u8;
  84         struct {
  85                 /* inverted: little endian */
  86                 uint8_t val_lsb:7;
  87                 uint8_t one:1;
  88         };
  89 };
  90
  91 SIGNAL(TIMER1_COMPA_vect)
  92 {
  93         PORTD = portval;
  94         TIMSK1 &= ~_BV(OCIE1A);
  95         done = 1;
  96 }
  97
  98 static void poll_spi(void)
  99 {
 100         uint8_t c;
 101         uint16_t servo;
 102         static union byte0 byte0_rx;
 103         union byte1 byte1_rx;
 104         union byte0 byte0_tx;
 105         static union byte1 byte1_tx;
 106
 107         /* reception complete ? */
 108         if (!(SPSR & (1<<SPIF)))
 109                 return;
 110
 111         c = SPDR;
 112
 113         /* prepare next TX */
 114
 115         if ((spi_out_idx & 1) == 0) {
 116                 servo = icp_servos[spi_out_idx >> 1];
 117                 byte0_tx.val_msb = servo >> 7;
 118                 byte0_tx.cmd_num = (spi_out_idx >> 1) + 1;
 119                 byte0_tx.zero = 0;
 120                 byte1_tx.val_lsb = servo & 0x7f;
 121                 byte1_tx.one = 1;
 122                 SPDR = byte0_tx.u8;
 123         }
 124         else {
 125                 SPDR = byte1_tx.u8;
 126         }
 127         spi_out_idx ++;
 128         if (spi_out_idx >= N_SERVO * 2)
 129                 spi_out_idx = 0;
 130
 131         /* RX */
 132
 133         if ((rxidx == 0) && (c & 0x80)) {
 134                 rxidx = 0;
 135                 return; /* drop */
 136         }
 137         if ((rxidx == 1) && ((c & 0x80) == 0)) {
 138                 rxidx = 0;
 139                 return; /* drop */
 140         }
 141
 142         if (rxidx == 0) {
 143                 byte0_rx.u8 = c;
 144
 145                 /* command num 0 is ignored */
 146                 if (byte0_rx.cmd_num == 0)
 147                         return;
 148         }
 149         else {
 150                 uint16_t val;
 151
 152                 byte1_rx.u8 = c;
 153
 154                 /* process command */
 155
 156                 val = (uint16_t)byte0_rx.val_msb << 7;
 157                 val += byte1_rx.val_lsb;
 158
 159                 if (byte0_rx.cmd_num < N_SERVO+1) {
 160                         servo_table[byte0_rx.cmd_num-1].command = val;
 161                 }
 162                 else if (byte0_rx.cmd_num == N_SERVO+1) {
 163                         if (val & PPM_BIT)
 164                                 ppm_enabled = 1;
 165                         else
 166                                 ppm_enabled = 0;
 167                         if (val & BYPASS_BIT)
 168                                 bypass = 1;
 169                         else
 170                                 bypass = 0;
 171                 }
 172         }
 173
 174         rxidx ^= 1;
 175 }
 176
 177 static void poll_input_capture(void)
 178 {
 179         uint16_t icp, diff;
 180
 181         /* no new sample, return */
 182         if ((TIFR1 & _BV(ICF1)) == 0)
 183                 return;
 184
 185         cli();
 186         icp = ICR1;
 187         sei();
 188
 189         /* clear the flag by writing a one */
 190         TIFR1 = TIFR1 | _BV(ICF1);
 191
 192         diff = icp - icp_prev;
 193         icp_prev = icp;
 194
 195         /* a rising edge with at least 2ms of state 0 means that we
 196          * get the first servo */
 197         if (diff > 3000) {
 198                 icp_idx = 0;
 199                 return;
 200         }
 201
 202         /* get the value for the servo */
 203         if (icp_idx < N_SERVO) {
 204                 if (diff < 1000)
 205                         icp_servos[icp_idx] = 0;
 206                 else if (diff > 2023)
 207                         icp_servos[icp_idx] = 1023;
 208                 else
 209                         icp_servos[icp_idx] = diff - 1000;
 210                 icp_idx++;
 211         }
 212 }
 213
 214 static void poll(void)
 215 {
 216         poll_spi();
 217         poll_input_capture();
 218 }
 219
 220 static void load_timer_at(uint16_t t)
 221 {
 222         OCR1A = t;
 223         TIMSK1 |= _BV(OCIE1A);
 224 }
 225
 226 static void do_servos(void)
 227 {
 228         uint8_t i;
 229         uint16_t t, start;
 230
 231         /* skip first servo if ppm is enabled */
 232         if (ppm_enabled)
 233                 i = 1;
 234         else
 235                 i = 0;
 236
 237         t = TCNT1;
 238         start = t + 100;
 239
 240         for (; i < N_SERVO; i++) {
 241
 242                 /* set servo and PPM bit */
 243                 portval = 1 << servo_table[i].bit;
 244                 if (ppm_enabled)
 245                         portval |= (1 << servo_table[PPM].bit);
 246
 247                 done = 0;
 248                 load_timer_at(start);
 249                 while (done == 0)
 250                         poll();
 251
 252                 /* reset PPM bit after 300us */
 253                 portval = 1 << servo_table[i].bit;
 254                 done = 0;
 255                 load_timer_at(start + 300);
 256                 while (done == 0)
 257                         poll();
 258
 259                 start = start + 1000 + servo_table[i].command;
 260         }
 261
 262         /* set PPM bit only for last servo */
 263         portval = 0;
 264         if (ppm_enabled)
 265                 portval |= (1 << servo_table[PPM].bit);
 266
 267         done = 0;
 268         load_timer_at(start);
 269         while (done == 0)
 270                 poll();
 271
 272         /* reset PPM bit after 300us */
 273         portval = 0;
 274         done = 0;
 275         load_timer_at(start + 300);
 276         while (done == 0)
 277                 poll();
 278 }
 279
 280 int main(void)
 281 {
 282         uint8_t t, diff;
 283         uint8_t tmp;
 284         uint8_t cnt = 10;
 285
 286         /* use pull-up for inputs */
 287         PORTC |= 0x3f;
 288
 289         /* LED */
 290         DDRB = 0x02;
 291
 292         while (cnt > 0) {
 293 #if 1 /* disable for LED debug only */
 294                 cnt--;
 295 #endif
 296                 LED_ON();
 297                 wait_ms(100);
 298                 LED_OFF();
 299                 wait_ms(100);
 300         }
 301
 302         /* servo outputs PD2-PD7 */
 303         DDRD = 0xfc;
 304
 305         /* start timer1 at clk/8 (1Mhz), enable noise canceler on
 306          * input capture, capture rising edge */
 307         TCNT1 = 0;
 308         TCCR1B = _BV(CS11) | _BV(ICNC1) | _BV(ICES1);
 309
 310         /* start timer0 at clk/1024 (~8Khz) */
 311         TCNT0 = 0;
 312         TCCR0B = _BV(CS02) | _BV(CS00);
 313
 314         /* enable spi (set MISO as output) */
 315         SPCR = _BV(SPE);
 316         SPDR = 0;
 317         DDRB |= _BV(4);
 318
 319         sei();
 320
 321         bypass = 0;
 322         while (1) {
 323                 t = TCNT0;
 324                 do_servos();
 325                 /* wait 20 ms */
 326                 while (1) {
 327                         diff = TCNT0 - t;
 328                         if (diff >= 160)
 329                                 break;
 330                         poll();
 331                 }
 332                 /* bypass mode */
 333                 if (bypass == 1) {
 334                         LED_ON();
 335
 336                         while (bypass == 1) {
 337                                 tmp = PINC;
 338                                 tmp &= 0x3f;
 339                                 tmp <<= 2;
 340                                 PORTD = tmp;
 341                                 poll();
 342                         }
 343                         LED_OFF();
 344                 }
 345         }
 346
 347         return 0;
 348 }