WebSVN - FlightCtrl - Diff - Rev 2108 and 2109 - /branches/dongfang_FC_fixedwing/arduino


#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 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) {
  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*) 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;
                debugOut.analog[28]++;
                break;
        }
}
 

Rev 2108	Rev 2109
1	#include "twimaster.h"	1	#include "twimaster.h"
2	#include "analog.h"	2	#include "analog.h"
-		3	#include "output.h"
3	#include <inttypes.h>	4	#include <inttypes.h>
4	#include <util/twi.h>	5	#include <util/twi.h>
5	#include <avr/interrupt.h>	6	#include <avr/interrupt.h>
6		7
7	uint8_t twiState;	8	uint8_t twiState;
8	uint8_t twiTroubleSpot;	9	uint8_t twiTroubleSpot;
9	uint8_t twiGotStatus;	10	uint8_t twiGotStatus;
10		11
11	volatile uint16_t IMU3200SensorInputs[4];	12	volatile uint16_t IMU3200SensorInputs[4];
12		13
13	void twimaster_init(void) {	14	void twimaster_init(void) {
14	// Set pullups	15	// Set pullups
15	uint8_t sreg = SREG;	16	uint8_t sreg = SREG;
16	cli();	17	cli();
17	DDRC = 0;	18	DDRC = 0;
18	PORTC = ((1 << 4) \| (1 << 5));	19	PORTC = ((1 << 4) \| (1 << 5));
19	TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;	20	TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2;
20	twiState = TWI_STATE_INIT_0;	21	twiState = TWI_STATE_INIT_0;
21	SREG = sreg;	22	SREG = sreg;
22	}	23	}
23		24
24	void I2CStart(void) {	25	void I2CStart(void) {
25	TWCR = (1 << TWINT) \| (1 << TWSTA) \| (1 << TWEN) \| (1 << TWIE);	26	TWCR = (1 << TWINT) \| (1 << TWSTA) \| (1 << TWEN) \| (1 << TWIE);
26	}	27	}
27		28
28	void I2CStop(void) {	29	void I2CStop(void) {
29	TWCR = (1 << TWINT) \| (1 << TWSTO) \| (1 << TWEN);	30	TWCR = (1 << TWINT) \| (1 << TWSTO) \| (1 << TWEN);
30	}	31	}
31		32
32	void I2CWriteByte(int8_t byte) {	33	void I2CWriteByte(int8_t byte) {
33	TWDR = byte;	34	TWDR = byte;
34	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE);	35	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE);
35	}	36	}
36		37
37	void I2CReceiveByte(void) {	38	void I2CReceiveByte(void) {
38	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE) \| (1 << TWEA);	39	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE) \| (1 << TWEA);
39	}	40	}
40		41
41	void I2CReceiveLastByte(void) {	42	void I2CReceiveLastByte(void) {
42	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE);	43	TWCR = (1 << TWINT) \| (1 << TWEN) \| (1 << TWIE);
43	}	44	}
44		45
45	void I2CReset(void) {	46	void I2CReset(void) {
-		47	debugOut.analog[29]++;
46	I2CStop();	48	I2CStop();
47	TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset	49	TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset
48	TWAMR = 0;	50	TWAMR = 0;
49	TWAR = 0;	51	TWAR = 0;
50	TWDR = 0;	52	TWDR = 0;
51	TWSR = 0;	53	TWSR = 0;
52	TWBR = 0;	54	TWBR = 0;
53	// Naaaah we dont need this: init_I2C();	55	// Naaaah we dont need this: init_I2C();
54	I2CStart();	56	I2CStart();
55	twiState = TWI_STATE_INIT_0;	57	twiState = TWI_STATE_INIT_0;
56	}	58	}
57		59
58	void twimaster_setNeutral(void) {	60	void twimaster_setNeutral(void) {
59	twiState = TWI_STATE_INIT_0;	61	twiState = TWI_STATE_INIT_0;
60	I2CStart();	62	I2CStart();
61	}	63	}
62		64
63	void twimaster_startCycle(void) {	65	void twimaster_startCycle(void) {
-		66	if (sensorDataReady & TWI_DATA_READY) { // if OK last time
64	twiState = TWI_STATE_LOOP_0;	67	twiState = TWI_STATE_LOOP_0;
65	I2CStart();	68	I2CStart();
-		69	} else { // go get em.
-		70	I2CReset();
-		71	}
66	}	72	}
67		73
68	const uint8_t EXPECTED_STATUS[] = { START, MT_SLA_ACK, MT_DATA_ACK,	74	const uint8_t EXPECTED_STATUS[] = { START, MT_SLA_ACK, MT_DATA_ACK,
69	MT_DATA_ACK, MT_DATA_ACK, REPEATED_START, MT_SLA_ACK, MT_DATA_ACK,	75	MT_DATA_ACK, MT_DATA_ACK, REPEATED_START, MT_SLA_ACK, MT_DATA_ACK,
70	REPEATED_START, MR_SLA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK,	76	REPEATED_START, MR_SLA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK,
71	MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_NACK };	77	MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_NACK };
72	const uint8_t GYRO_CONFIGURATION[] = { 15, 3 << 3 };	78	const uint8_t GYRO_CONFIGURATION[] = { 15, 3 << 3 };
73		79
74	ISR( TWI_vect) {	80	ISR( TWI_vect) {
75	if (twiState == sizeof(EXPECTED_STATUS)) {	81	if (twiState == sizeof(EXPECTED_STATUS)) {
76	// We have come too far, panic!	82	// We have come too far, panic!
77	I2CReset();	83	I2CReset();
78	}	84	}
79		85
80	uint8_t cur_twi_state = twiState;	86	uint8_t cur_twi_state = twiState;
81	twiState++;	87	twiState++;
82	uint8_t status = TWSR_FILTER;	88	uint8_t status = TWSR_FILTER;
83	if (status != EXPECTED_STATUS[cur_twi_state]) {	89	if (status != EXPECTED_STATUS[cur_twi_state]) {
84	// A little confusion between repeated and nonrepeated start is OK.	90	// A little confusion between repeated and nonrepeated start is OK.
85	if (!((status == START && EXPECTED_STATUS[cur_twi_state]	91	if (!((status == START && EXPECTED_STATUS[cur_twi_state]
86	== REPEATED_START) \|\| (status == REPEATED_START	92	== REPEATED_START) \|\| (status == REPEATED_START
87	&& EXPECTED_STATUS[cur_twi_state] == START))) {	93	&& EXPECTED_STATUS[cur_twi_state] == START))) {
88	// Panic, reset	94	// Panic, reset
89	twiTroubleSpot = cur_twi_state;	95	twiTroubleSpot = cur_twi_state;
90	twiGotStatus = status;	96	twiGotStatus = status;
91	I2CReset();	97	I2CReset();
92	}	98	}
93	}	99	}
94		100
95	uint8_t dataIndex = cur_twi_state - TWI_STATE_LOOP_0 - 5;	101	uint8_t dataIndex = cur_twi_state - TWI_STATE_LOOP_0 - 5;
96	// Fix endiannness!	102	// Fix endiannness!
97	if (dataIndex & 1)	103	if (dataIndex & 1)
98	dataIndex &= ~1;	104	dataIndex &= ~1;
99	else	105	else
100	dataIndex \|= 1;	106	dataIndex \|= 1;
101		107
102	switch (cur_twi_state) {	108	switch (cur_twi_state) {
103	case TWI_STATE_INIT_0:	109	case TWI_STATE_INIT_0:
104	I2CWriteByte((SLA << 1) \| 0);	110	I2CWriteByte((SLA << 1) \| 0);
105	break;	111	break;
106	case TWI_STATE_INIT_0 + 1:	112	case TWI_STATE_INIT_0 + 1:
107	I2CWriteByte(GYRO_CONFIGURATION_START);	113	I2CWriteByte(GYRO_CONFIGURATION_START);
108	break;	114	break;
109	case TWI_STATE_INIT_0 + 2:	115	case TWI_STATE_INIT_0 + 2:
110	I2CWriteByte(GYRO_CONFIGURATION[0]);	116	I2CWriteByte(GYRO_CONFIGURATION[0]);
111	break;	117	break;
112	case TWI_STATE_INIT_0 + 3:	118	case TWI_STATE_INIT_0 + 3:
113	I2CWriteByte(GYRO_CONFIGURATION[1]);	119	I2CWriteByte(GYRO_CONFIGURATION[1]);
114	break;	120	break;
115	case TWI_STATE_INIT_0 + 4:	121	case TWI_STATE_INIT_0 + 4:
116	I2CStop();	122	I2CStop();
117	// Need to check ACK ans return to 0 if not (skipping start below).	123	// Need to check ACK ans return to 0 if not (skipping start below).
118	I2CStart();	124	I2CStart();
119	break;	125	break;
120	case TWI_STATE_LOOP_0:	126	case TWI_STATE_LOOP_0:
121	I2CWriteByte((SLA << 1) \| 0);	127	I2CWriteByte((SLA << 1) \| 0);
122	break;	128	break;
123	case TWI_STATE_LOOP_0 + 1:	129	case TWI_STATE_LOOP_0 + 1:
124	I2CWriteByte(GYRO_DATA_START);	130	I2CWriteByte(GYRO_DATA_START);
125	break;	131	break;
126	case TWI_STATE_LOOP_0 + 2:	132	case TWI_STATE_LOOP_0 + 2:
127	I2CStop();	133	I2CStop();
128	I2CStart();	134	I2CStart();
129	break;	135	break;
130	case TWI_STATE_LOOP_0 + 3:	136	case TWI_STATE_LOOP_0 + 3:
131	I2CWriteByte((SLA << 1) \| 1);	137	I2CWriteByte((SLA << 1) \| 1);
132	break;	138	break;
133	case TWI_STATE_LOOP_0 + 4:	139	case TWI_STATE_LOOP_0 + 4:
134	I2CReceiveByte();	140	I2CReceiveByte();
135	break;	141	break;
136	default: // data bytes	142	default: // data bytes
137	((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR;	143	((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR;
138	if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1)	144	if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1)
139	I2CReceiveByte();	145	I2CReceiveByte();
140	else	146	else
141	I2CReceiveLastByte();	147	I2CReceiveLastByte();
142	break;	148	break;
143	case TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1: // last data byte	149	case TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1: // last data byte
144	((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR;	150	((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR;
145	// Dont re-init the gyro but just restart the loop.	151	// Dont re-init the gyro but just restart the loop.
146	I2CStop();	152	I2CStop();
147	sensorDataReady \|= TWI_DATA_READY;	153	sensorDataReady \|= TWI_DATA_READY;
-		154	debugOut.analog[28]++;
148	break;	155	break;
149	}	156	}
150	}	157	}
151		158

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/twimaster.c - Rev 2108 → 2109