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