cmdline: new rc_proto commands
[protos/xbee-avr.git] / spi_servo.c
1 #include <stdint.h>
2 #include <stdio.h>
3
4 #include <aversive.h>
5 #include <aversive/wait.h>
6
7 #include <callout.h>
8
9 #include "spi_servo.h"
10 #include "main.h"
11
12 /*
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
18  *
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.
25  */
26
27 #define PPM_BIT    0x01
28 #define BYPASS_BIT 0x02
29
30 struct spi_servo_tx {
31         uint16_t servo[N_SERVO+1]; /* one more for control channel */
32         uint16_t cmd_mask;
33         uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
34         uint8_t cur_idx;
35 };
36 static struct spi_servo_tx spi_servo_tx;
37
38 struct spi_servo_rx {
39         uint16_t servo[N_SERVO];
40         uint8_t prev_byte;
41 };
42 static struct spi_servo_rx spi_servo_rx;
43
44 static struct callout spi_timer;
45
46 /*
47  * SPI protocol:
48  *
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.
52  *
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
58  */
59 union spi_byte0 {
60         uint8_t u8;
61         struct {
62                 /* inverted: little endian */
63                 uint8_t val_msb:3;
64                 uint8_t cmd_num:4;
65                 uint8_t zero:1;
66         };
67 };
68
69 union spi_byte1 {
70         uint8_t u8;
71         struct {
72                 /* inverted: little endian */
73                 uint8_t val_lsb:7;
74                 uint8_t one:1;
75         };
76 };
77
78 #define SS_HIGH() PORTB |= (1 << 4)
79 #define SS_LOW() PORTB &= (~(1 << 4))
80
81 static void spi_send_byte(uint8_t byte)
82 {
83         SS_LOW();
84         SPDR = byte;
85         /* Wait for transmission complete (active loop is fine because
86          * the clock is high) */
87         while(!(SPSR & (1<<SPIF)));
88         SS_HIGH();
89 }
90
91 static void spi_send_one_servo(uint8_t num, uint16_t val)
92 {
93         union spi_byte0 byte0;
94         union spi_byte1 byte1;
95
96         byte0.val_msb = val >> 7;
97         byte0.cmd_num = num + 1;
98         byte0.zero = 0;
99         byte1.one = 1;
100         byte1.val_lsb = val;
101
102         /* save the second byte */
103         spi_servo_tx.next_byte = byte1.u8;
104
105         /* send the first byte */
106         spi_send_byte(byte0.u8);
107 }
108
109 static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
110 {
111         uint8_t num;
112         uint16_t val;
113
114         num = byte0.cmd_num - 1;
115         if (num >= N_SERVO)
116                 return;
117
118         val = byte0.val_msb;
119         val <<= 7;
120         val |= byte1.val_lsb;
121
122         spi_servo_rx.servo[num] = val;
123 }
124
125 /* called by the scheduler */
126 static void spi_servo_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
127 {
128         uint8_t idx;
129         union spi_byte0 byte0;
130         union spi_byte1 byte1;
131
132         (void)arg;
133
134         /* get the value from the slave */
135         byte0.u8 = SPDR;
136         byte1.u8 = byte0.u8;
137         if (byte0.zero == 0) {
138                 spi_servo_rx.prev_byte = byte0.u8;
139         }
140         else {
141                 byte0.u8 = spi_servo_rx.prev_byte;
142                 decode_rx_servo(byte0, byte1);
143         }
144
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;
149                 goto reschedule;
150         }
151
152         /* if there is no updated servo, send 0 and return. */
153         if (spi_servo_tx.cmd_mask == 0) {
154                 spi_send_byte(0);
155                 goto reschedule;
156         }
157
158         /* else find it and send it */
159         idx = spi_servo_tx.cur_idx;
160         while (1) {
161                 idx++;
162                 if (idx > N_SERVO + 1)
163                         idx = 0;
164
165                 if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
166                         break;
167         }
168
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;
172
173  reschedule:
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);
177 }
178
179 void spi_servo_init(void)
180 {
181         /* SCK, SS & MOSI */
182         DDRB = 0xb0;
183
184         /* remove power reduction on spi */
185         PRR0 &= ~(1 << PRSPI);
186
187         /* Enable SPI, Master, set clock rate fck/16 */
188         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0);
189
190         SS_HIGH();
191
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);
195 }
196
197 void spi_servo_set(uint8_t num, uint16_t val)
198 {
199         uint8_t flags;
200
201         if (num >= N_SERVO)
202                 return;
203
204         IRQ_LOCK(flags);
205         spi_servo_tx.servo[num] = val;
206         spi_servo_tx.cmd_mask |= (1 << num);
207         IRQ_UNLOCK(flags);
208 }
209
210 uint16_t spi_servo_get(uint8_t num)
211 {
212         uint8_t flags;
213         uint16_t val;
214
215         if (num >= N_SERVO)
216                 return 0;
217
218         IRQ_LOCK(flags);
219         val = spi_servo_rx.servo[num];
220         IRQ_UNLOCK(flags);
221
222         return val;
223 }
224
225 uint8_t spi_servo_get_bypass(void)
226 {
227         return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
228 }
229
230 uint8_t spi_servo_get_ppm(void)
231 {
232         return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
233 }
234
235 void spi_servo_set_bypass(uint8_t enable)
236 {
237         uint8_t flags;
238
239         IRQ_LOCK(flags);
240         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
241         if (enable)
242                 spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
243         else
244                 spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
245         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
246         IRQ_UNLOCK(flags);
247 }
248
249 void spi_servo_set_ppm(uint8_t enable)
250 {
251         uint8_t flags;
252
253         IRQ_LOCK(flags);
254         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
255         if (enable)
256                 spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
257         else
258                 spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
259         spi_servo_tx.cmd_mask |= (1 << N_SERVO);
260         IRQ_UNLOCK(flags);
261 }
262
263 void spi_servo_dump(void)
264 {
265         uint8_t i;
266
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());
272 }