Rev 2109 | Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2108 | - | 1 | #include "twimaster.h" |
2 | #include "analog.h" |
||
3 | #include <inttypes.h> |
||
4 | #include <util/twi.h> |
||
5 | #include <avr/interrupt.h> |
||
6 | |||
7 | uint8_t twiState; |
||
8 | uint8_t twiTroubleSpot; |
||
9 | uint8_t twiGotStatus; |
||
10 | |||
11 | volatile uint16_t IMU3200SensorInputs[4]; |
||
12 | |||
13 | void twimaster_init(void) { |
||
14 | // Set pullups |
||
15 | uint8_t sreg = SREG; |
||
16 | cli(); |
||
17 | DDRC = 0; |
||
18 | PORTC = ((1 << 4) | (1 << 5)); |
||
19 | TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2; |
||
20 | twiState = TWI_STATE_INIT_0; |
||
21 | SREG = sreg; |
||
22 | } |
||
23 | |||
24 | void I2CStart(void) { |
||
25 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
||
26 | } |
||
27 | |||
28 | void I2CStop(void) { |
||
29 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
||
30 | } |
||
31 | |||
32 | void I2CWriteByte(int8_t byte) { |
||
33 | TWDR = byte; |
||
34 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
||
35 | } |
||
36 | |||
37 | void I2CReceiveByte(void) { |
||
38 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
||
39 | } |
||
40 | |||
41 | void I2CReceiveLastByte(void) { |
||
42 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
||
43 | } |
||
44 | |||
45 | void I2CReset(void) { |
||
46 | I2CStop(); |
||
47 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
||
48 | TWAMR = 0; |
||
49 | TWAR = 0; |
||
50 | TWDR = 0; |
||
51 | TWSR = 0; |
||
52 | TWBR = 0; |
||
53 | // Naaaah we dont need this: init_I2C(); |
||
54 | I2CStart(); |
||
55 | twiState = TWI_STATE_INIT_0; |
||
56 | } |
||
57 | |||
58 | void twimaster_setNeutral(void) { |
||
59 | twiState = TWI_STATE_INIT_0; |
||
60 | I2CStart(); |
||
61 | } |
||
62 | |||
63 | void twimaster_startCycle(void) { |
||
64 | twiState = TWI_STATE_LOOP_0; |
||
65 | I2CStart(); |
||
66 | } |
||
67 | |||
68 | const uint8_t EXPECTED_STATUS[] = { START, MT_SLA_ACK, MT_DATA_ACK, |
||
69 | MT_DATA_ACK, MT_DATA_ACK, REPEATED_START, MT_SLA_ACK, MT_DATA_ACK, |
||
70 | REPEATED_START, MR_SLA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, |
||
71 | MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_NACK }; |
||
72 | const uint8_t GYRO_CONFIGURATION[] = { 15, 3 << 3 }; |
||
73 | |||
74 | ISR( TWI_vect) { |
||
75 | if (twiState == sizeof(EXPECTED_STATUS)) { |
||
76 | // We have come too far, panic! |
||
77 | I2CReset(); |
||
78 | } |
||
79 | |||
80 | uint8_t cur_twi_state = twiState; |
||
81 | twiState++; |
||
82 | uint8_t status = TWSR_FILTER; |
||
83 | if (status != EXPECTED_STATUS[cur_twi_state]) { |
||
84 | // A little confusion between repeated and nonrepeated start is OK. |
||
85 | if (!((status == START && EXPECTED_STATUS[cur_twi_state] |
||
86 | == REPEATED_START) || (status == REPEATED_START |
||
87 | && EXPECTED_STATUS[cur_twi_state] == START))) { |
||
88 | // Panic, reset |
||
89 | twiTroubleSpot = cur_twi_state; |
||
90 | twiGotStatus = status; |
||
91 | I2CReset(); |
||
92 | } |
||
93 | } |
||
94 | |||
95 | uint8_t dataIndex = cur_twi_state - TWI_STATE_LOOP_0 - 5; |
||
96 | // Fix endiannness! |
||
97 | if (dataIndex & 1) |
||
98 | dataIndex &= ~1; |
||
99 | else |
||
100 | dataIndex |= 1; |
||
101 | |||
102 | switch (cur_twi_state) { |
||
103 | case TWI_STATE_INIT_0: |
||
104 | I2CWriteByte((SLA << 1) | 0); |
||
105 | break; |
||
106 | case TWI_STATE_INIT_0 + 1: |
||
107 | I2CWriteByte(GYRO_CONFIGURATION_START); |
||
108 | break; |
||
109 | case TWI_STATE_INIT_0 + 2: |
||
110 | I2CWriteByte(GYRO_CONFIGURATION[0]); |
||
111 | break; |
||
112 | case TWI_STATE_INIT_0 + 3: |
||
113 | I2CWriteByte(GYRO_CONFIGURATION[1]); |
||
114 | break; |
||
115 | case TWI_STATE_INIT_0 + 4: |
||
116 | I2CStop(); |
||
117 | // Need to check ACK ans return to 0 if not (skipping start below). |
||
118 | I2CStart(); |
||
119 | break; |
||
120 | case TWI_STATE_LOOP_0: |
||
121 | I2CWriteByte((SLA << 1) | 0); |
||
122 | break; |
||
123 | case TWI_STATE_LOOP_0 + 1: |
||
124 | I2CWriteByte(GYRO_DATA_START); |
||
125 | break; |
||
126 | case TWI_STATE_LOOP_0 + 2: |
||
127 | I2CStop(); |
||
128 | I2CStart(); |
||
129 | break; |
||
130 | case TWI_STATE_LOOP_0 + 3: |
||
131 | I2CWriteByte((SLA << 1) | 1); |
||
132 | break; |
||
133 | case TWI_STATE_LOOP_0 + 4: |
||
134 | I2CReceiveByte(); |
||
135 | break; |
||
136 | default: // data bytes |
||
137 | ((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
||
138 | if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1) |
||
139 | I2CReceiveByte(); |
||
140 | else |
||
141 | I2CReceiveLastByte(); |
||
142 | break; |
||
143 | case TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1: // last data byte |
||
144 | ((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
||
145 | // Dont re-init the gyro but just restart the loop. |
||
146 | I2CStop(); |
||
147 | sensorDataReady |= TWI_DATA_READY; |
||
148 | break; |
||
149 | } |
||
150 | } |