Subversion Repositories FlightCtrl

#include <string.h>
#include <avr/io.h>
#include <util/twi.h>
#include <avr/interrupt.h>
#include "servoboard.h"
#include "twislave.h"

uint8_t I2C_RXBuffer[32];
uint8_t Byte_Counter = 0;
uint8_t I2C_timeout = 0;

void i2c_init(uint8_t adr)
{
    TWAR = adr;
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
}

uint8_t calc_crc(uint8_t *buffer, uint8_t l) {
        uint8_t crc = 0xff;
        uint8_t i;
        for(i = 0; i < l; i++) {
                crc = crc ^ buffer[i];
        }
        return crc;
}

ISR(TWI_vect) {

    switch (TWSR & 0xF8) {  
        case SR_SLA_ACK:  
            TWCR |= (1<<TWINT);
            Byte_Counter = 0;
            return;
        case SR_PREV_ACK:
                        I2C_timeout = 3;
                        if (Byte_Counter < 32) {
                                I2C_RXBuffer[Byte_Counter++] = TWDR;
                        }
                        if (Byte_Counter == 7) {
                                if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) {
                                        memcpy(pwm_position, I2C_RXBuffer, 6);
                                }
                        }
            TWCR |= (1<<TWINT);
            return;
        case TWI_BUS_ERR_2:
            TWCR |= (1<<TWSTO) | (1<<TWINT);
        case TWI_BUS_ERR_1:
            TWCR |= (1<<TWSTO) | (1<<TWINT);
    }
    TWCR = (1 << TWEA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE);

}