+
+ /* prepare next TX */
+
+ if ((spi_out_idx & 1) == 0) {
+ servo = icp_servos[spi_out_idx >> 1];
+ byte0_tx.val_msb = servo >> 7;
+ byte0_tx.cmd_num = (spi_out_idx >> 1) + 1;
+ byte0_tx.zero = 0;
+ byte1_tx.val_lsb = servo & 0x7f;
+ byte1_tx.one = 1;
+ SPDR = byte0_tx.u8;
+ }
+ else {
+ SPDR = byte1_tx.u8;
+ }
+ spi_out_idx ++;
+ if (spi_out_idx >= NB_SERVO)
+ spi_out_idx = 0;
+
+ /* RX */
+