Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1455 acid 1
 
2
#include <string.h>
3
#include <avr/io.h>
4
#include <util/twi.h>
5
#include <avr/interrupt.h>
6
#include "servoboard.h"
7
#include "twislave.h"
8
 
1456 acid 9
uint8_t I2C_RXBuffer[32];
10
uint8_t Byte_Counter = 0;
1475 acid 11
volatile uint8_t I2C_timeout = 0;
1455 acid 12
 
1459 acid 13
void i2c_init(uint8_t adr)
1455 acid 14
{
15
    TWAR = adr;
16
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
17
}
18
 
19
uint8_t calc_crc(uint8_t *buffer, uint8_t l) {
20
        uint8_t crc = 0xff;
21
        uint8_t i;
22
        for(i = 0; i < l; i++) {
23
                crc = crc ^ buffer[i];
24
        }
25
        return crc;
26
}
27
 
1459 acid 28
ISR(TWI_vect) {
1455 acid 29
 
1456 acid 30
    switch (TWSR & 0xF8) {  
1455 acid 31
        case SR_SLA_ACK:  
32
            TWCR |= (1<<TWINT);
33
            Byte_Counter = 0;
34
            return;
35
        case SR_PREV_ACK:
1477 acid 36
                        I2C_timeout = 5;
1455 acid 37
                        if (Byte_Counter < 32) {
38
                                I2C_RXBuffer[Byte_Counter++] = TWDR;
39
                        }
1459 acid 40
                        if (Byte_Counter == 7) {
1455 acid 41
                                if (calc_crc(&I2C_RXBuffer, 6) == I2C_RXBuffer[6]) {
42
                                        memcpy(pwm_position, I2C_RXBuffer, 6);
43
                                }
44
                        }
45
            TWCR |= (1<<TWINT);
46
            return;
47
        case TWI_BUS_ERR_2:
1459 acid 48
            TWCR |= (1<<TWSTO) | (1<<TWINT);
1455 acid 49
        case TWI_BUS_ERR_1:
1459 acid 50
            TWCR |= (1<<TWSTO) | (1<<TWINT);
1456 acid 51
    }
1459 acid 52
    TWCR = (1 << TWEA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE);
1456 acid 53
 
1455 acid 54
}
55
 
56