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 BYPASS_BIT 0x02
31 uint16_t servo[N_SERVO+1]; /* one more for control channel */
33 uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
36 static struct spi_servo_tx spi_servo_tx;
39 uint16_t servo[N_SERVO];
42 static struct spi_servo_rx spi_servo_rx;
44 static struct callout spi_timer;
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.
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
62 /* inverted: little endian */
72 /* inverted: little endian */
78 #define SS_HIGH() PORTB |= (1 << 4)
79 #define SS_LOW() PORTB &= (~(1 << 4))
81 static void spi_send_byte(uint8_t byte)
85 /* Wait for transmission complete (active loop is fine because
86 * the clock is high) */
87 while(!(SPSR & (1<<SPIF)));
91 static void spi_send_one_servo(uint8_t num, uint16_t val)
93 union spi_byte0 byte0;
94 union spi_byte1 byte1;
96 byte0.val_msb = val >> 7;
97 byte0.cmd_num = num + 1;
102 /* save the second byte */
103 spi_servo_tx.next_byte = byte1.u8;
105 /* send the first byte */
106 spi_send_byte(byte0.u8);
109 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
114 num = byte0.cmd_num - 1;
120 val |= byte1.val_lsb;
122 spi_servo_rx.servo[num] = val;
125 /* called by the scheduler */
126 static void spi_servo_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
129 union spi_byte0 byte0;
130 union spi_byte1 byte1;
134 /* get the value from the slave */
137 if (byte0.zero == 0) {
138 spi_servo_rx.prev_byte = byte0.u8;
141 byte0.u8 = spi_servo_rx.prev_byte;
142 decode_rx_servo(byte0, byte1);
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;
152 /* if there is no updated servo, send 0 and return. */
153 if (spi_servo_tx.cmd_mask == 0) {
158 /* else find it and send it */
159 idx = spi_servo_tx.cur_idx;
162 if (idx > N_SERVO + 1)
165 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
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;
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);
179 void spi_servo_init(void)
184 /* remove power reduction on spi */
185 PRR0 &= ~(1 << PRSPI);
187 /* Enable SPI, Master, set clock rate fck/16 */
188 SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0);
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);
197 void spi_servo_set(uint8_t num, uint16_t val)
205 spi_servo_tx.servo[num] = val;
206 spi_servo_tx.cmd_mask |= (1 << num);
210 uint16_t spi_servo_get(uint8_t num)
219 val = spi_servo_rx.servo[num];
225 uint8_t spi_servo_get_bypass(void)
227 return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
230 uint8_t spi_servo_get_ppm(void)
232 return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
235 void spi_servo_set_bypass(uint8_t enable)
240 spi_servo_tx.cmd_mask |= (1 << N_SERVO);
242 spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
244 spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
245 spi_servo_tx.cmd_mask |= (1 << N_SERVO);
249 void spi_servo_set_ppm(uint8_t enable)
254 spi_servo_tx.cmd_mask |= (1 << N_SERVO);
256 spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
258 spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
259 spi_servo_tx.cmd_mask |= (1 << N_SERVO);
263 void spi_servo_dump(void)
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());