Rev 1452 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1452 | Rev 1568 | ||
---|---|---|---|
1 | /*############################################################################ |
1 | /*############################################################################ |
2 | ############################################################################*/ |
2 | ############################################################################*/ |
3 | 3 | ||
4 | #include "main.h" |
4 | #include "main.h" |
- | 5 | #include "heli.h" |
|
5 | 6 | ||
6 | volatile unsigned char twi_state = 0; |
7 | volatile unsigned char twi_state = 0; |
7 | unsigned char motor = 0; |
8 | unsigned char motor = 0; |
8 | unsigned char motorread = 0,MissingMotor = 0; |
9 | unsigned char motorread = 0,MissingMotor = 0; |
9 | unsigned char motor_rx[16],motor_rx2[16]; |
10 | unsigned char motor_rx[16],motor_rx2[16]; |
10 | unsigned char MotorPresent[MAX_MOTORS]; |
11 | unsigned char MotorPresent[MAX_MOTORS]; |
11 | unsigned char MotorError[MAX_MOTORS]; |
12 | unsigned char MotorError[MAX_MOTORS]; |
12 | unsigned int I2CError = 0; |
13 | unsigned int I2CError = 0; |
13 | 14 | ||
14 | //############################################################################ |
15 | //############################################################################ |
15 | //Initzialisieren der I2C (TWI) Schnittstelle |
16 | //Initzialisieren der I2C (TWI) Schnittstelle |
16 | void i2c_init(void) |
17 | void i2c_init(void) |
17 | //############################################################################ |
18 | //############################################################################ |
18 | { |
19 | { |
19 | TWSR = 0; |
20 | TWSR = 0; |
20 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
21 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
21 | } |
22 | } |
22 | 23 | ||
23 | //############################################################################ |
24 | //############################################################################ |
24 | //Start I2C |
25 | //Start I2C |
25 | void i2c_start(void) |
26 | void i2c_start(void) |
26 | //############################################################################ |
27 | //############################################################################ |
27 | { |
28 | { |
28 | TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
29 | TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
29 | } |
30 | } |
30 | 31 | ||
31 | //############################################################################ |
32 | //############################################################################ |
32 | void i2c_stop(void) |
33 | void i2c_stop(void) |
33 | //############################################################################ |
34 | //############################################################################ |
34 | { |
35 | { |
35 | TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
36 | TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
36 | } |
37 | } |
37 | 38 | ||
38 | void i2c_reset(void) |
39 | void i2c_reset(void) |
39 | //############################################################################ |
40 | //############################################################################ |
40 | { |
41 | { |
41 | i2c_stop(); |
42 | i2c_stop(); |
42 | twi_state = 0; |
43 | twi_state = 0; |
43 | motor = TWDR; |
44 | motor = TWDR; |
44 | motor = 0; |
45 | motor = 0; |
45 | TWCR = 0x80; |
46 | TWCR = 0x80; |
46 | TWAMR = 0; |
47 | TWAMR = 0; |
47 | TWAR = 0; |
48 | TWAR = 0; |
48 | TWDR = 0; |
49 | TWDR = 0; |
49 | TWSR = 0; |
50 | TWSR = 0; |
50 | TWBR = 0; |
51 | TWBR = 0; |
51 | i2c_init(); |
52 | i2c_init(); |
52 | i2c_start(); |
53 | i2c_start(); |
53 | i2c_write_byte(0); |
54 | i2c_write_byte(0); |
54 | } |
55 | } |
55 | 56 | ||
56 | //############################################################################ |
57 | //############################################################################ |
57 | void i2c_write_byte(char byte) |
58 | void i2c_write_byte(char byte) |
58 | //############################################################################ |
59 | //############################################################################ |
59 | { |
60 | { |
60 | TWSR = 0x00; |
61 | TWSR = 0x00; |
61 | TWDR = byte; |
62 | TWDR = byte; |
62 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
63 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
63 | } |
64 | } |
64 | 65 | ||
65 | /****************************************/ |
66 | /****************************************/ |
66 | /* Write to I2C */ |
67 | /* Write to I2C */ |
67 | /****************************************/ |
68 | /****************************************/ |
68 | void I2C_WriteByte(int8_t byte) |
69 | void I2C_WriteByte(int8_t byte) |
69 | { |
70 | { |
70 | // move byte to send into TWI Data Register |
71 | // move byte to send into TWI Data Register |
71 | TWDR = byte; |
72 | TWDR = byte; |
72 | // clear interrupt flag (TWINT = 1) |
73 | // clear interrupt flag (TWINT = 1) |
73 | // enable i2c bus (TWEN = 1) |
74 | // enable i2c bus (TWEN = 1) |
74 | // enable interrupt (TWIE = 1) |
75 | // enable interrupt (TWIE = 1) |
75 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
76 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
76 | } |
77 | } |
77 | 78 | ||
78 | /****************************************/ |
79 | /****************************************/ |
79 | /* Receive byte and send ACK */ |
80 | /* Receive byte and send ACK */ |
80 | /****************************************/ |
81 | /****************************************/ |
81 | void I2C_ReceiveByte(void) |
82 | void I2C_ReceiveByte(void) |
82 | { |
83 | { |
83 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
84 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
84 | } |
85 | } |
85 | 86 | ||
86 | /****************************************/ |
87 | /****************************************/ |
87 | /* I2C receive last byte and send no ACK*/ |
88 | /* I2C receive last byte and send no ACK*/ |
88 | /****************************************/ |
89 | /****************************************/ |
89 | void I2C_ReceiveLastByte(void) |
90 | void I2C_ReceiveLastByte(void) |
90 | { |
91 | { |
91 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
92 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
92 | } |
93 | } |
93 | 94 | ||
94 | uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
95 | uint8_t calc_crc(uint8_t *buffer, uint8_t l) { |
95 | uint8_t crc = 0xff; |
96 | uint8_t crc = 0xff; |
96 | uint8_t i; |
97 | uint8_t i; |
97 | for(i = 0; i < l; i++) { |
98 | for(i = 0; i < l; i++) { |
98 | crc = crc ^ buffer[i]; |
99 | crc = crc ^ buffer[i]; |
99 | } |
100 | } |
100 | return crc; |
101 | return crc; |
101 | } |
102 | } |
102 | 103 | ||
103 | //############################################################################ |
104 | //############################################################################ |
104 | SIGNAL (TWI_vect) |
105 | SIGNAL (TWI_vect) |
105 | //############################################################################ |
106 | //############################################################################ |
106 | { |
107 | { |
107 | static unsigned char missing_motor; |
108 | static unsigned char missing_motor; |
108 | switch(twi_state++) |
109 | switch(twi_state++) |
109 | { |
110 | { |
110 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
111 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
111 | // writing Gyro-Offset |
112 | // writing Gyro-Offset |
112 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
113 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
113 | case 8: |
114 | case 8: |
114 | i2c_write_byte(0x98); // Address of the DAC |
115 | i2c_write_byte(0x98); // Address of the DAC |
115 | break; |
116 | break; |
116 | case 9: |
117 | case 9: |
117 | i2c_write_byte(0x10); // Update Channel A |
118 | i2c_write_byte(0x10); // Update Channel A |
118 | break; |
119 | break; |
119 | case 10: |
120 | case 10: |
120 | i2c_write_byte(AnalogOffsetNick); // Value |
121 | i2c_write_byte(AnalogOffsetNick); // Value |
121 | break; |
122 | break; |
122 | case 11: |
123 | case 11: |
123 | i2c_write_byte(0x80); // Value |
124 | i2c_write_byte(0x80); // Value |
124 | break; |
125 | break; |
125 | case 12: |
126 | case 12: |
126 | i2c_stop(); |
127 | i2c_stop(); |
127 | I2CTimeout = 10; |
128 | I2CTimeout = 10; |
128 | i2c_start(); |
129 | i2c_start(); |
129 | break; |
130 | break; |
130 | case 13: |
131 | case 13: |
131 | i2c_write_byte(0x98); // Address of the DAC |
132 | i2c_write_byte(0x98); // Address of the DAC |
132 | break; |
133 | break; |
133 | case 14: |
134 | case 14: |
134 | i2c_write_byte(0x12); // Update Channel B |
135 | i2c_write_byte(0x12); // Update Channel B |
135 | break; |
136 | break; |
136 | case 15: |
137 | case 15: |
137 | i2c_write_byte(AnalogOffsetRoll); // Value |
138 | i2c_write_byte(AnalogOffsetRoll); // Value |
138 | break; |
139 | break; |
139 | case 16: |
140 | case 16: |
140 | i2c_write_byte(0x80); // Value |
141 | i2c_write_byte(0x80); // Value |
141 | break; |
142 | break; |
142 | case 17: |
143 | case 17: |
143 | i2c_stop(); |
144 | i2c_stop(); |
144 | I2CTimeout = 10; |
145 | I2CTimeout = 10; |
145 | i2c_start(); |
146 | i2c_start(); |
146 | break; |
147 | break; |
147 | case 18: |
148 | case 18: |
148 | i2c_write_byte(0x98); // Address of the DAC |
149 | i2c_write_byte(0x98); // Address of the DAC |
149 | break; |
150 | break; |
150 | case 19: |
151 | case 19: |
151 | i2c_write_byte(0x14); // Update Channel C |
152 | i2c_write_byte(0x14); // Update Channel C |
152 | break; |
153 | break; |
153 | case 20: |
154 | case 20: |
154 | i2c_write_byte(AnalogOffsetGier); // Value |
155 | i2c_write_byte(AnalogOffsetGier); // Value |
155 | break; |
156 | break; |
156 | case 21: |
157 | case 21: |
157 | i2c_write_byte(0x80); // Value |
158 | i2c_write_byte(0x80); // Value |
158 | break; |
159 | break; |
159 | case 22: |
160 | case 22: |
160 | i2c_stop(); |
161 | i2c_stop(); |
161 | I2CTimeout = 10; |
162 | I2CTimeout = 10; |
162 | twi_state = 0; |
163 | twi_state = 0; |
163 | break; |
164 | break; |
164 | 165 | ||
165 | case 0: |
166 | case 0: |
166 | i2c_write_byte(0x82); // servo board |
- | |
167 | twi_state = 100; |
- | |
168 | break; |
- | |
169 | case 100: // servo 1 |
- | |
170 | i2c_write_byte(servoValues[0]); |
- | |
171 | break; |
- | |
172 | case 101: // servo 2 |
- | |
173 | i2c_write_byte(servoValues[1]); |
- | |
174 | break; |
- | |
175 | case 102: // servo 3 |
- | |
176 | i2c_write_byte(servoValues[2]); |
- | |
177 | break; |
- | |
178 | case 103: // servo 4 |
- | |
179 | i2c_write_byte(servoValues[3]); |
- | |
180 | break; |
- | |
181 | case 104: // servo 5 |
- | |
182 | i2c_write_byte(servoValues[4]); |
- | |
183 | break; |
- | |
184 | case 105: // servo 6 |
- | |
185 | i2c_write_byte(servoValues[5]); |
- | |
186 | break; |
- | |
187 | case 106: |
- | |
188 | i2c_write_byte(calc_crc(&servoValues, 6)); |
- | |
189 | break; |
- | |
190 | case 107: |
- | |
191 | i2c_stop(); |
167 | i2c_stop(); |
192 | I2CTimeout = 10; |
168 | I2CTimeout = 10; |
193 | twi_state = 0; |
169 | twi_state = 0; |
194 | break; |
170 | break; |
195 | default: twi_state = 0; |
171 | default: twi_state = 0; |
196 | break; |
172 | break; |
197 | } |
173 | } |
198 | TWCR |= 0x80; |
174 | TWCR |= 0x80; |
199 | } |
175 | } |
200 | 176 |