Subversion Repositories FlightCtrl

#include "twimaster.h"
#include "analog.h"
#include "output.h"
#include <inttypes.h>
#include <util/twi.h>
#include <avr/interrupt.h>

uint8_t twiState;
uint8_t twiTroubleSpot;
uint8_t twiGotStatus;

volatile int16_t ITG3200SensorInputs[4];

void twimaster_init(void) {
        // Set pullups OFF
        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) {
  debugOut.analog[29]++;
        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) {
        if (sensorDataReady & TWI_DATA_READY) { // if OK last time
          twiState = TWI_STATE_LOOP_0;
          I2CStart();
        } else {                                // go get em.
          I2CReset();
        }
}

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*) ITG3200SensorInputs)[dataIndex] = TWDR;
                if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(ITG3200SensorInputs) - 1)
                        I2CReceiveByte();
                else
                        I2CReceiveLastByte();
                break;
        case TWI_STATE_LOOP_0 + 5 + sizeof(ITG3200SensorInputs) - 1: // last data byte
                ((uint8_t*) ITG3200SensorInputs)[dataIndex] = TWDR;
                // Dont re-init the gyro but just restart the loop.
                I2CStop();
                sensorDataReady |= TWI_DATA_READY;
                debugOut.analog[28]++;
                break;
        }
}