5 #include <aversive/wait.h>
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
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.
28 #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
31 #define BYPASS_ENABLE 14
32 #define BYPASS_DISABLE 15
34 uint16_t servo[N_SERVO];
36 uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
39 static struct spi_servo_tx spi_servo_tx;
42 uint16_t servo[N_SERVO];
45 static struct spi_servo_rx spi_servo_rx;
50 * A command is stored on 2 bytes. The first one has its msb to 0, and the
51 * second one to 1. The first received byte contains the command number, and the
52 * msb of the servo value. The second byte contains the lsb of the servo value.
54 * Commands 0 to NB_SERVO are to set the value of servo.
55 * Command 14 is to enable bypass mode.
56 * Command 15 is to disable bypass mode.
61 /* inverted: little endian */
71 /* inverted: little endian */
77 #define SS_HIGH() PORTB |= (1 << 4)
78 #define SS_LOW() PORTB &= (~(1 << 4))
80 static void spi_send_byte(uint8_t byte)
84 /* Wait for transmission complete (active loop is fine because
85 * the clock is high) */
86 while(!(SPSR & (1<<SPIF)));
90 static void spi_send_one_servo(uint8_t num, uint16_t val)
92 union spi_byte0 byte0;
93 union spi_byte1 byte1;
95 byte0.val_msb = val >> 7;
97 byte0.cmd_num = num + 1;
104 /* save the second byte */
105 spi_servo_tx.next_byte = byte1.u8;
107 /* send the first byte */
108 spi_send_byte(byte0.u8);
111 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
116 num = byte0.cmd_num - 1;
122 val |= byte1.val_lsb;
124 spi_servo_rx.servo[num] = val;
127 /* called by the scheduler */
128 static void spi_servo_cb(void *arg)
131 union spi_byte0 byte0;
132 union spi_byte1 byte1;
136 /* get the value from the slave */
139 if (byte0.zero == 0) {
140 spi_servo_rx.prev_byte = byte0.u8;
143 byte0.u8 = spi_servo_rx.prev_byte;
144 decode_rx_servo(byte0, byte1);
147 /* if next byte is set, send it */
148 if (spi_servo_tx.next_byte != 0) {
149 spi_send_byte(spi_servo_tx.next_byte);
150 spi_servo_tx.next_byte = 0;
154 /* if there is no updated servo, send 0 and return. */
155 if (spi_servo_tx.cmd_mask == 0) {
160 /* else find it and send it */
161 idx = spi_servo_tx.cur_idx;
166 else if (idx >= sizeof(uint16_t) * 8)
169 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
173 spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
174 spi_servo_tx.cmd_mask &= (~(1 << idx));
175 spi_servo_tx.cur_idx = idx;
178 void spi_servo_init(void)
183 /* remove power reduction on spi */
184 PRR0 &= ~(1 << PRSPI);
186 /* Enable SPI, Master, set clock rate fck/64 */
187 SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
191 scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
192 SPI_EVT_PERIOD, SPI_PRIO);
193 spi_servo_set(BYPASS_DISABLE, 0);
196 void spi_servo_set(uint8_t num, uint16_t val)
204 spi_servo_tx.servo[num] = val;
205 spi_servo_tx.cmd_mask |= (1 << num);
209 uint16_t spi_servo_get(uint8_t num)
218 val = spi_servo_rx.servo[num];
224 void spi_servo_bypass(uint8_t enable)
230 spi_servo_tx.cmd_mask |= (1 << BYPASS_ENABLE);
235 spi_servo_tx.cmd_mask |= (1 << BYPASS_DISABLE);