| 0,0 → 1,150 |
|---|
| #include "twimaster.h" |
| #include "analog.h" |
| #include <inttypes.h> |
| #include <util/twi.h> |
| #include <avr/interrupt.h> |
| |
| uint8_t twiState; |
| uint8_t twiTroubleSpot; |
| uint8_t twiGotStatus; |
| |
| volatile uint16_t IMU3200SensorInputs[4]; |
| |
| void twimaster_init(void) { |
| // Set pullups |
| uint8_t sreg = SREG; |
| cli(); |
| DDRC = 0; |
| PORTC = ((1 << 4) | (1 << 5)); |
| TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2; |
| twiState = TWI_STATE_INIT_0; |
| SREG = sreg; |
| } |
| |
| void I2CStart(void) { |
| TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
| } |
| |
| void I2CStop(void) { |
| TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
| } |
| |
| void I2CWriteByte(int8_t byte) { |
| TWDR = byte; |
| TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
| } |
| |
| void I2CReceiveByte(void) { |
| TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
| } |
| |
| void I2CReceiveLastByte(void) { |
| TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
| } |
| |
| void I2CReset(void) { |
| I2CStop(); |
| TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
| TWAMR = 0; |
| TWAR = 0; |
| TWDR = 0; |
| TWSR = 0; |
| TWBR = 0; |
| // Naaaah we dont need this: init_I2C(); |
| I2CStart(); |
| twiState = TWI_STATE_INIT_0; |
| } |
| |
| void twimaster_setNeutral(void) { |
| twiState = TWI_STATE_INIT_0; |
| I2CStart(); |
| } |
| |
| void twimaster_startCycle(void) { |
| twiState = TWI_STATE_LOOP_0; |
| I2CStart(); |
| } |
| |
| const uint8_t EXPECTED_STATUS[] = { START, MT_SLA_ACK, MT_DATA_ACK, |
| MT_DATA_ACK, MT_DATA_ACK, REPEATED_START, MT_SLA_ACK, MT_DATA_ACK, |
| REPEATED_START, MR_SLA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, |
| MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_NACK }; |
| const uint8_t GYRO_CONFIGURATION[] = { 15, 3 << 3 }; |
| |
| ISR( TWI_vect) { |
| if (twiState == sizeof(EXPECTED_STATUS)) { |
| // We have come too far, panic! |
| I2CReset(); |
| } |
| |
| uint8_t cur_twi_state = twiState; |
| twiState++; |
| uint8_t status = TWSR_FILTER; |
| if (status != EXPECTED_STATUS[cur_twi_state]) { |
| // A little confusion between repeated and nonrepeated start is OK. |
| if (!((status == START && EXPECTED_STATUS[cur_twi_state] |
| == REPEATED_START) || (status == REPEATED_START |
| && EXPECTED_STATUS[cur_twi_state] == START))) { |
| // Panic, reset |
| twiTroubleSpot = cur_twi_state; |
| twiGotStatus = status; |
| I2CReset(); |
| } |
| } |
| |
| uint8_t dataIndex = cur_twi_state - TWI_STATE_LOOP_0 - 5; |
| // Fix endiannness! |
| if (dataIndex & 1) |
| dataIndex &= ~1; |
| else |
| dataIndex |= 1; |
| |
| switch (cur_twi_state) { |
| case TWI_STATE_INIT_0: |
| I2CWriteByte((SLA << 1) | 0); |
| break; |
| case TWI_STATE_INIT_0 + 1: |
| I2CWriteByte(GYRO_CONFIGURATION_START); |
| break; |
| case TWI_STATE_INIT_0 + 2: |
| I2CWriteByte(GYRO_CONFIGURATION[0]); |
| break; |
| case TWI_STATE_INIT_0 + 3: |
| I2CWriteByte(GYRO_CONFIGURATION[1]); |
| break; |
| case TWI_STATE_INIT_0 + 4: |
| I2CStop(); |
| // Need to check ACK ans return to 0 if not (skipping start below). |
| I2CStart(); |
| break; |
| case TWI_STATE_LOOP_0: |
| I2CWriteByte((SLA << 1) | 0); |
| break; |
| case TWI_STATE_LOOP_0 + 1: |
| I2CWriteByte(GYRO_DATA_START); |
| break; |
| case TWI_STATE_LOOP_0 + 2: |
| I2CStop(); |
| I2CStart(); |
| break; |
| case TWI_STATE_LOOP_0 + 3: |
| I2CWriteByte((SLA << 1) | 1); |
| break; |
| case TWI_STATE_LOOP_0 + 4: |
| I2CReceiveByte(); |
| break; |
| default: // data bytes |
| ((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
| if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1) |
| I2CReceiveByte(); |
| else |
| I2CReceiveLastByte(); |
| break; |
| case TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1: // last data byte |
| ((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
| // Dont re-init the gyro but just restart the loop. |
| I2CStop(); |
| sensorDataReady |= TWI_DATA_READY; |
| break; |
| } |
| } |