Subversion Repositories FlightCtrl

Rev

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
}