fix scheduler unit as we have a 12Mhz quartz
[protos/xbee-avr.git] / spi_servo.c
1 #include <aversive.h>
2 #include <aversive/wait.h>
3 #include <stdint.h>
4 #include <stdio.h>
5
6 #include "spi_servo.h"
7
8 #define BYPASS_ENABLE 14
9 #define BYPASS_DISABLE 15
10
11 /*
12  * SPI protocol:
13  *
14  * A command is stored on 2 bytes. The first one has its msb to 0, and the
15  * second one to 1. The first received byte contains the command number, and the
16  * msb of the servo value. The second byte contains the lsb of the servo value.
17  *
18  * Commands 0 to NB_SERVO are to set the value of servo.
19  * Command 14 is to enable bypass mode.
20  * Command 15 is to disable bypass mode.
21  */
22 static volatile union {
23         uint8_t u8;
24         struct {
25                 /* inverted: little endian */
26                 uint8_t val_msb:3;
27                 uint8_t cmd_num:4;
28                 uint8_t zero:1;
29         };
30 } byte0;
31
32 static volatile union {
33         uint8_t u8;
34         struct {
35                 /* inverted: little endian */
36                 uint8_t val_lsb:7;
37                 uint8_t one:1;
38         };
39 } byte1;
40
41 #define SS_HIGH() PORTB |= (1 << 4)
42 #define SS_LOW() PORTB &= (~(1 << 4))
43
44 void spi_servo_init(void)
45 {
46         /* SCK, SS & MOSI */
47         DDRB = 0xb0;
48
49         /* remove power reduction on spi */
50         PRR0 &= ~(1 << PRSPI);
51
52         /* Enable SPI, Master, set clock rate fck/64 */
53         SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
54
55         SS_HIGH();
56
57         spi_servo_set(BYPASS_DISABLE, 0);
58 }
59
60 void spi_servo_set(uint8_t num, uint16_t val)
61 {
62         byte0.val_msb = val >> 7;
63         byte0.cmd_num = num;
64         byte0.zero = 0;
65         byte1.one = 1;
66         byte1.val_lsb = val;
67
68         SS_LOW();
69         SPDR = byte0.u8;
70         /* Wait for transmission complete */
71         while(!(SPSR & (1<<SPIF)));
72         SS_HIGH();
73
74         _delay_loop_1(5);
75         SS_LOW();
76
77         SPDR = byte1.u8;
78         /* Wait for transmission complete */
79         while(!(SPSR & (1<<SPIF)));
80         SS_HIGH();
81 }
82
83 void spi_servo_bypass(uint8_t enable)
84 {
85         if (enable)
86                 spi_servo_set(BYPASS_ENABLE, 0);
87         else
88                 spi_servo_set(BYPASS_DISABLE, 0);
89 }