+ uint8_t c;
+
+ /* reception complete ? */
+ if (!(SPSR & (1<<SPIF)))
+ return;
+
+ c = SPDR;
+ if ((rxidx == 0) && (c & 0x80)) {
+ rxidx = 0;
+ return; /* drop */
+ }
+ if ((rxidx == 1) && ((c & 0x80) == 0)) {
+ rxidx = 0;
+ return; /* drop */
+ }
+
+ if (rxidx == 0) {
+ byte0.u8 = c;
+ }
+ else {
+ uint16_t val;
+
+ byte1.u8 = c;
+
+ /* process command */
+
+ if (byte0.cmd_num < NB_SERVO) {
+ val = (uint16_t)byte0.val_msb << 7;
+ val += byte1.val_lsb;
+ servo_table[byte0.cmd_num].command = val;
+ }
+ else if (byte0.cmd_num == 14) {
+ bypass = 1;
+ }
+ else if (byte0.cmd_num == 15) {
+ bypass = 0;
+ }
+ }
+
+ rxidx ^= 1;