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 | } |