Rev 935 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 935 | Rev 936 | ||
|---|---|---|---|
| Line 5... | Line 5... | ||
| 5 | #include <avr/interrupt.h> |
5 | #include <avr/interrupt.h> |
| Line 6... | Line 6... | ||
| 6 | 6 | ||
| 7 | #include "main.h" |
7 | #include "main.h" |
| 8 | #include "twimaster.h" |
8 | #include "twimaster.h" |
| - | 9 | #include "fc.h" |
|
| Line 9... | Line 10... | ||
| 9 | #include "fc.h" |
10 | #include "analog.h" |
| - | 11 | ||
| 10 | 12 | volatile uint8_t twi_state = 0; |
|
| - | 13 | volatile uint8_t motor_write = 0; |
|
| 11 | volatile uint8_t twi_state = 0; |
14 | volatile uint8_t motor_read = 0; |
| - | 15 | volatile uint8_t dac_channel = 0; |
|
| - | 16 | volatile uint8_t motor_rx[8]; |
|
| - | 17 | volatile uint16_t I2CTimeout = 100; |
|
| - | 18 | ||
| - | 19 | ||
| - | 20 | #define SCL_CLOCK 200000L |
|
| - | 21 | #define I2C_TIMEOUT 30000 |
|
| - | 22 | ||
| - | 23 | #define TWSR_STATUS_MASK 0xF8 |
|
| - | 24 | // for Master Transmitter Mode |
|
| - | 25 | ||
| - | 26 | #define I2C_STATUS_START 0x08 |
|
| - | 27 | #define I2C_STATUS_REPEATSTART 0x10 |
|
| - | 28 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
|
| - | 29 | #define I2C_STATUS_SLAW_NOACK 0x20 |
|
| - | 30 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
|
| - | 31 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
|
| Line 12... | Line 32... | ||
| 12 | volatile uint8_t motor = 0; |
32 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
| 13 | volatile uint8_t motor_rx[8]; |
33 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
| 14 | 34 | ||
| 15 | /**************************************************/ |
35 | /**************************************************/ |
| Line 32... | Line 52... | ||
| 32 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
52 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
| Line 33... | Line 53... | ||
| 33 | 53 | ||
| 34 | // set TWI Bit Rate Register |
54 | // set TWI Bit Rate Register |
| Line -... | Line 55... | ||
| - | 55 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
|
| - | 56 | ||
| - | 57 | twi_state = 0; |
|
| - | 58 | motor_write = 0; |
|
| 35 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
59 | motor_read = 0; |
| 36 | 60 | ||
| Line 37... | Line 61... | ||
| 37 | SREG = sreg; |
61 | SREG = sreg; |
| 38 | } |
62 | } |
| Line 46... | Line 70... | ||
| 46 | // clear TWI interrupt flag (TWINT=1) |
70 | // clear TWI interrupt flag (TWINT=1) |
| 47 | // disable TWI Acknowledge Bit (TWEA = 0) |
71 | // disable TWI Acknowledge Bit (TWEA = 0) |
| 48 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
72 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
| 49 | // disable TWI STOP Condition Bit (TWSTO = 0) |
73 | // disable TWI STOP Condition Bit (TWSTO = 0) |
| 50 | // disable TWI Write Collision Flag (TWWC = 0) |
74 | // disable TWI Write Collision Flag (TWWC = 0) |
| 51 | // enable i2c (TWIE = 1) |
75 | // enable i2c (TWEN = 1) |
| 52 | // enable TWI Interrupt (TWIE = 1) |
76 | // enable TWI Interrupt (TWIE = 1) |
| 53 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
77 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
| 54 | } |
78 | } |
| Line 55... | Line 79... | ||
| 55 | 79 | ||
| Line 62... | Line 86... | ||
| 62 | // clear TWI interrupt flag (TWINT=1) |
86 | // clear TWI interrupt flag (TWINT=1) |
| 63 | // disable TWI Acknowledge Bit (TWEA = 0) |
87 | // disable TWI Acknowledge Bit (TWEA = 0) |
| 64 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
88 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
| 65 | // enable TWI STOP Condition Bit (TWSTO = 1) |
89 | // enable TWI STOP Condition Bit (TWSTO = 1) |
| 66 | // disable TWI Write Collision Flag (TWWC = 0) |
90 | // disable TWI Write Collision Flag (TWWC = 0) |
| 67 | // enable i2c (TWIE = 1) |
91 | // enable i2c (TWEN = 1) |
| 68 | // disable TWI Interrupt (TWIE = 0) |
92 | // disable TWI Interrupt (TWIE = 0) |
| 69 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
93 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
| 70 | } |
94 | } |
| Line 71... | Line -... | ||
| 71 | - | ||
| 72 | /****************************************/ |
- | |
| 73 | /* Reset I2C */ |
- | |
| 74 | /****************************************/ |
- | |
| 75 | void I2C_Reset(void) |
- | |
| 76 | { |
- | |
| 77 | // stop i2c bus |
- | |
| 78 | I2C_Stop(); |
- | |
| 79 | twi_state = 0; |
- | |
| 80 | motor = TWDR; // ?? |
- | |
| 81 | motor = 0; |
- | |
| 82 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
- | |
| 83 | TWAMR = 0; |
- | |
| 84 | TWAR = 0; |
- | |
| 85 | TWDR = 0; |
- | |
| 86 | TWSR = 0; |
- | |
| 87 | TWBR = 0; |
- | |
| 88 | I2C_Init(); |
- | |
| 89 | I2C_Start(); |
- | |
| 90 | I2C_WriteByte(0); |
- | |
| Line 91... | Line 95... | ||
| 91 | } |
95 | |
| 92 | 96 | ||
| 93 | /****************************************/ |
97 | /****************************************/ |
| 94 | /* Write to I2C */ |
98 | /* Write to I2C */ |
| 95 | /****************************************/ |
99 | /****************************************/ |
| 96 | void I2C_WriteByte(int8_t byte) |
100 | void I2C_WriteByte(int8_t byte) |
| 97 | { |
101 | { |
| 98 | // move byte to send into TWI Data Register |
102 | // move byte to send into TWI Data Register |
| 99 | TWDR = byte; |
103 | TWDR = byte; |
| 100 | // clear interrupt flag (TWINT = 1) |
104 | // clear interrupt flag (TWINT = 1) |
| 101 | // enable i2c bus (TWEN = 1) |
105 | // enable i2c bus (TWEN = 1) |
| 102 | // enable intterupt (TWIW = 1) |
106 | // enable interrupt (TWIE = 1) |
| Line 103... | Line 107... | ||
| 103 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
107 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| Line 120... | Line 124... | ||
| 120 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
124 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| 121 | } |
125 | } |
| Line 122... | Line 126... | ||
| 122 | 126 | ||
| - | 127 | ||
| - | 128 | /****************************************/ |
|
| - | 129 | /* Reset I2C */ |
|
| - | 130 | /****************************************/ |
|
| - | 131 | void I2C_Reset(void) |
|
| - | 132 | { |
|
| - | 133 | // stop i2c bus |
|
| - | 134 | I2C_Stop(); |
|
| - | 135 | twi_state = 0; |
|
| - | 136 | motor_write = TWDR; |
|
| - | 137 | motor_write = 0; |
|
| - | 138 | motor_read = 0; |
|
| - | 139 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
|
| - | 140 | TWAMR = 0; |
|
| - | 141 | TWAR = 0; |
|
| - | 142 | TWDR = 0; |
|
| - | 143 | TWSR = 0; |
|
| - | 144 | TWBR = 0; |
|
| - | 145 | I2C_Init(); |
|
| - | 146 | I2C_Start(); |
|
| - | 147 | I2C_WriteByte(0); |
|
| - | 148 | } |
|
| 123 | 149 | ||
| 124 | /****************************************/ |
150 | /****************************************/ |
| 125 | /* I2C ISR */ |
151 | /* I2C ISR */ |
| Line 126... | Line 152... | ||
| 126 | /****************************************/ |
152 | /****************************************/ |
| 127 | ISR (TWI_vect) |
- | |
| Line 128... | Line 153... | ||
| 128 | 153 | ISR (TWI_vect) |
|
| 129 | { |
154 | |
| 130 | static uint8_t motorread = 0; |
155 | { |
| 131 | 156 | ||
| 132 | switch (twi_state++) // First i2s_start from SendMotorData() |
157 | switch (twi_state++) // First i2c_start from SendMotorData() |
| 133 | { |
158 | { |
| 134 | // Master Transmit |
159 | // Master Transmit |
| 135 | case 0: // Send SLA-W |
160 | case 0: // Send SLA-W |
| 136 | I2C_WriteByte(0x52+(motor*2)); |
161 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
| 137 | break; |
162 | break; |
| 138 | case 1: // Send Data to Salve |
163 | case 1: // Send Data to Slave |
| 139 | switch(motor++) |
164 | switch(motor_write) |
| 140 | { |
165 | { |
| 141 | case 0: |
166 | case 0: |
| Line 148... | Line 173... | ||
| 148 | I2C_WriteByte(Motor_Right); |
173 | I2C_WriteByte(Motor_Right); |
| 149 | break; |
174 | break; |
| 150 | case 3: |
175 | case 3: |
| 151 | I2C_WriteByte(Motor_Left); |
176 | I2C_WriteByte(Motor_Left); |
| 152 | break; |
177 | break; |
| 153 | } |
178 | } |
| 154 | break; |
179 | break; |
| 155 | case 2: // repeat case 0+1 for all Slaves |
180 | case 2: // repeat case 0+1 for all motors |
| - | 181 | I2C_Stop(); |
|
| 156 | if (motor<4) twi_state = 0; |
182 | if (motor_write < 3) |
| - | 183 | { |
|
| - | 184 | motor_write++; // jump to next motor |
|
| - | 185 | twi_state = 0; // and repeat from state 0 |
|
| - | 186 | } |
|
| - | 187 | else |
|
| - | 188 | { // data to last motor send |
|
| - | 189 | motor_write = 0; // reset motor write counter |
|
| - | 190 | } |
|
| 157 | I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive |
191 | I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive |
| 158 | break; |
192 | break; |
| Line 159... | Line 193... | ||
| 159 | 193 | ||
| 160 | // Master Receive |
194 | // Master Receive |
| 161 | case 3: // Send SLA-R |
195 | case 3: // Send SLA-R |
| 162 | I2C_WriteByte(0x53+(motorread*2)); |
196 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
| 163 | break; |
197 | break; |
| 164 | case 4: |
198 | case 4: |
| 165 | //Transmit 1st byte |
199 | //Transmit 1st byte |
| 166 | I2C_ReceiveByte(); |
200 | I2C_ReceiveByte(); |
| 167 | break; |
201 | break; |
| 168 | case 5: //Read 1st byte and transmit 2nd Byte |
202 | case 5: //Read 1st byte and transmit 2nd Byte |
| 169 | motor_rx[motorread] = TWDR; |
203 | motor_rx[motor_read] = TWDR; |
| 170 | I2C_ReceiveLastByte(); |
204 | I2C_ReceiveLastByte(); |
| 171 | break; |
205 | break; |
| 172 | case 6: |
206 | case 6: |
| 173 | //Read 2nd byte |
207 | //Read 2nd byte |
| 174 | motor_rx[motorread+4] = TWDR; |
208 | motor_rx[motor_read + 4] = TWDR; |
| 175 | motorread++; |
209 | motor_read++; |
| - | 210 | if (motor_read > 3) motor_read = 0; |
|
| - | 211 | I2C_Stop(); |
|
| - | 212 | twi_state = 0; |
|
| - | 213 | I2CTimeout = 10; |
|
| - | 214 | break; |
|
| - | 215 | ||
| - | 216 | // Gyro-Offsets |
|
| - | 217 | case 7: |
|
| - | 218 | I2C_WriteByte(0x98); // Address the DAC |
|
| - | 219 | break; |
|
| - | 220 | ||
| - | 221 | case 8: |
|
| - | 222 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
|
| - | 223 | break; |
|
| - | 224 | ||
| - | 225 | case 9: |
|
| - | 226 | switch(dac_channel) |
|
| - | 227 | { |
|
| - | 228 | case 0: |
|
| - | 229 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
|
| - | 230 | break; |
|
| - | 231 | case 1: |
|
| - | 232 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
|
| - | 233 | break; |
|
| - | 234 | case 2: |
|
| - | 235 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
|
| - | 236 | break; |
|
| - | 237 | } |
|
| - | 238 | break; |
|
| - | 239 | ||
| - | 240 | case 10: |
|
| - | 241 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
|
| - | 242 | break; |
|
| - | 243 | ||
| - | 244 | case 11: |
|
| - | 245 | I2C_Stop(); |
|
| - | 246 | I2CTimeout = 10; |
|
| - | 247 | // repeat case 7...10 until all DAC Channels are updated |
|
| - | 248 | if(dac_channel < 2) |
|
| - | 249 | { |
|
| - | 250 | dac_channel ++; // jump to next channel |
|
| - | 251 | twi_state = 7; // and repeat from state 7 |
|
| - | 252 | I2C_Start(); // start transmission for next channel |
|
| - | 253 | } |
|
| - | 254 | else |
|
| - | 255 | { // data to last motor send |
|
| - | 256 | dac_channel = 0; // reset dac channel counter |
|
| - | 257 | twi_state = 0; // reset twi_state |
|
| - | 258 | } |
|
| Line 176... | Line 259... | ||
| 176 | if (motorread > 3) motorread=0; |
259 | break; |
| 177 | 260 | ||
| 178 | default: |
261 | default: |
| 179 | I2C_Stop(); |
262 | I2C_Stop(); |
| - | 263 | twi_state = 0; |
|
| 180 | twi_state = 0; |
264 | I2CTimeout = 10; |
| 181 | I2CTimeout = 10; |
265 | motor_write = 0; |
| 182 | motor = 0; |
266 | motor_read = 0; |