Rev 2035 | Rev 2158 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2035 | Rev 2097 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
2 | // + Copyright (c) 04.2007 Holger Buss |
- | |
3 | // + Nur f�r den privaten Gebrauch |
- | |
4 | // + www.MikroKopter.com |
- | |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
- | |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist. |
- | |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
- | |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
- | |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
- | |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
- | |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
- | |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
- | |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
- | |
17 | // + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
- | |
18 | // + eindeutig als Ursprung verlinkt werden |
- | |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
- | |
21 | // + Benutzung auf eigene Gefahr |
- | |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
- | |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
- | |
25 | // + mit unserer Zustimmung zul�ssig |
- | |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
- | |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
- | |
30 | // + this list of conditions and the following disclaimer. |
- | |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
- | |
32 | // + from this software without specific prior written permission. |
- | |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
- | |
34 | // + for non-commercial use (directly or indirectly) |
- | |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
- | |
36 | // + with our written permission |
- | |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
- | |
38 | // + clearly linked as origin |
- | |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
- | |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
- | |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
- | |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
- | |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
- | |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
- | |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
- | |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
- | |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
- | |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
- | |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
- | |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
51 | - | ||
52 | #include <avr/io.h> |
1 | #include <avr/io.h> |
53 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
54 | #include <util/twi.h> |
3 | #include <util/twi.h> |
55 | #include <util/delay.h> |
4 | #include <util/delay.h> |
56 | #include "twimaster.h" |
5 | #include "twimaster.h" |
57 | #include "configuration.h" |
6 | #include "configuration.h" |
58 | #include "analog.h" |
7 | #include "analog.h" |
59 | #include "printf_P.h" |
8 | #include "printf_P.h" |
60 | 9 | ||
61 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
10 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
62 | volatile uint8_t dac_channel = 0; |
11 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t motor_write = 0; |
12 | volatile uint8_t motor_write = 0; |
64 | volatile uint8_t motor_read = 0; |
13 | volatile uint8_t motor_read = 0; |
65 | volatile uint16_t I2CTimeout = 100; |
14 | volatile uint16_t I2CTimeout = 100; |
66 | uint8_t missingMotor = 0; |
15 | uint8_t missingMotor = 0; |
67 | 16 | ||
68 | motorData_t motor[MAX_MOTORS]; |
17 | motorData_t motor[MAX_MOTORS]; |
69 | 18 | ||
70 | uint8_t DACChannel = 0; |
19 | uint8_t DACChannel = 0; |
71 | 20 | ||
72 | #define SCL_CLOCK 200000L |
21 | #define SCL_CLOCK 200000L |
73 | #define I2C_TIMEOUT 30000 |
22 | #define I2C_TIMEOUT 30000 |
74 | 23 | ||
75 | /************************************************** |
24 | /************************************************** |
76 | * Initialize I2C (TWI) |
25 | * Initialize I2C (TWI) |
77 | **************************************************/ |
26 | **************************************************/ |
78 | void I2C_init(void) { |
27 | void I2C_init(void) { |
79 | uint8_t i; |
28 | uint8_t i; |
80 | uint8_t sreg = SREG; |
29 | uint8_t sreg = SREG; |
81 | cli(); |
30 | cli(); |
82 | 31 | ||
83 | // SDA is INPUT |
32 | // SDA is INPUT |
84 | DDRC &= ~(1 << DDC1); |
33 | DDRC &= ~(1 << DDC1); |
85 | // SCL is output |
34 | // SCL is output |
86 | DDRC |= (1 << DDC0); |
35 | DDRC |= (1 << DDC0); |
87 | // pull up SDA |
36 | // pull up SDA |
88 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
37 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
89 | 38 | ||
90 | // TWI Status Register |
39 | // TWI Status Register |
91 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
40 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
92 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
41 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
93 | 42 | ||
94 | // set TWI Bit Rate Register |
43 | // set TWI Bit Rate Register |
95 | TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
44 | TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
96 | 45 | ||
97 | twi_state = TWI_STATE_MOTOR_TX; |
46 | twi_state = TWI_STATE_MOTOR_TX; |
98 | motor_write = 0; |
47 | motor_write = 0; |
99 | motor_read = 0; |
48 | motor_read = 0; |
100 | 49 | ||
101 | for (i = 0; i < MAX_MOTORS; i++) { |
50 | for (i = 0; i < MAX_MOTORS; i++) { |
102 | motor[i].throttle = 0; |
51 | motor[i].throttle = 0; |
103 | motor[i].present = 0; |
52 | motor[i].present = 0; |
104 | motor[i].maxPWM = 0; |
53 | motor[i].maxPWM = 0; |
105 | } |
54 | } |
106 | 55 | ||
107 | SREG = sreg; |
56 | SREG = sreg; |
108 | } |
57 | } |
109 | 58 | ||
110 | /**************************************** |
59 | /**************************************** |
111 | * Start I2C |
60 | * Start I2C |
112 | ****************************************/ |
61 | ****************************************/ |
113 | void I2C_Start(uint8_t start_state) { |
62 | void I2C_Start(uint8_t start_state) { |
114 | twi_state = start_state; |
63 | twi_state = start_state; |
115 | // TWI Control Register |
64 | // TWI Control Register |
116 | // clear TWI interrupt flag (TWINT=1) |
65 | // clear TWI interrupt flag (TWINT=1) |
117 | // disable TWI Acknowledge Bit (TWEA = 0) |
66 | // disable TWI Acknowledge Bit (TWEA = 0) |
118 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
67 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
119 | // disable TWI STOP Condition Bit (TWSTO = 0) |
68 | // disable TWI STOP Condition Bit (TWSTO = 0) |
120 | // disable TWI Write Collision Flag (TWWC = 0) |
69 | // disable TWI Write Collision Flag (TWWC = 0) |
121 | // enable i2c (TWEN = 1) |
70 | // enable i2c (TWEN = 1) |
122 | // enable TWI Interrupt (TWIE = 1) |
71 | // enable TWI Interrupt (TWIE = 1) |
123 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
72 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
124 | } |
73 | } |
125 | 74 | ||
126 | /**************************************** |
75 | /**************************************** |
127 | * Stop I2C |
76 | * Stop I2C |
128 | ****************************************/ |
77 | ****************************************/ |
129 | void I2C_Stop(uint8_t start_state) { |
78 | void I2C_Stop(uint8_t start_state) { |
130 | twi_state = start_state; |
79 | twi_state = start_state; |
131 | // TWI Control Register |
80 | // TWI Control Register |
132 | // clear TWI interrupt flag (TWINT=1) |
81 | // clear TWI interrupt flag (TWINT=1) |
133 | // disable TWI Acknowledge Bit (TWEA = 0) |
82 | // disable TWI Acknowledge Bit (TWEA = 0) |
134 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
83 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
135 | // enable TWI STOP Condition Bit (TWSTO = 1) |
84 | // enable TWI STOP Condition Bit (TWSTO = 1) |
136 | // disable TWI Write Collision Flag (TWWC = 0) |
85 | // disable TWI Write Collision Flag (TWWC = 0) |
137 | // enable i2c (TWEN = 1) |
86 | // enable i2c (TWEN = 1) |
138 | // disable TWI Interrupt (TWIE = 0) |
87 | // disable TWI Interrupt (TWIE = 0) |
139 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
88 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
140 | } |
89 | } |
141 | 90 | ||
142 | /**************************************** |
91 | /**************************************** |
143 | * Write to I2C |
92 | * Write to I2C |
144 | ****************************************/ |
93 | ****************************************/ |
145 | void I2C_WriteByte(int8_t byte) { |
94 | void I2C_WriteByte(int8_t byte) { |
146 | // move byte to send into TWI Data Register |
95 | // move byte to send into TWI Data Register |
147 | TWDR = byte; |
96 | TWDR = byte; |
148 | // clear interrupt flag (TWINT = 1) |
97 | // clear interrupt flag (TWINT = 1) |
149 | // enable i2c bus (TWEN = 1) |
98 | // enable i2c bus (TWEN = 1) |
150 | // enable interrupt (TWIE = 1) |
99 | // enable interrupt (TWIE = 1) |
151 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
100 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
152 | } |
101 | } |
153 | 102 | ||
154 | /**************************************** |
103 | /**************************************** |
155 | * Receive byte and send ACK |
104 | * Receive byte and send ACK |
156 | ****************************************/ |
105 | ****************************************/ |
157 | void I2C_ReceiveByte(void) { |
106 | void I2C_ReceiveByte(void) { |
158 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
107 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
159 | } |
108 | } |
160 | 109 | ||
161 | /**************************************** |
110 | /**************************************** |
162 | * I2C receive last byte and send no ACK |
111 | * I2C receive last byte and send no ACK |
163 | ****************************************/ |
112 | ****************************************/ |
164 | void I2C_ReceiveLastByte(void) { |
113 | void I2C_ReceiveLastByte(void) { |
165 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
114 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
166 | } |
115 | } |
167 | 116 | ||
168 | /**************************************** |
117 | /**************************************** |
169 | * Reset I2C |
118 | * Reset I2C |
170 | ****************************************/ |
119 | ****************************************/ |
171 | void I2C_Reset(void) { |
120 | void I2C_Reset(void) { |
172 | // stop i2c bus |
121 | // stop i2c bus |
173 | I2C_Stop(TWI_STATE_MOTOR_TX); |
122 | I2C_Stop(TWI_STATE_MOTOR_TX); |
174 | twi_state = 0; |
123 | twi_state = 0; |
175 | motor_write = TWDR; |
124 | motor_write = TWDR; |
176 | motor_write = 0; |
125 | motor_write = 0; |
177 | motor_read = 0; |
126 | motor_read = 0; |
178 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
127 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
179 | TWAMR = 0; |
128 | TWAMR = 0; |
180 | TWAR = 0; |
129 | TWAR = 0; |
181 | TWDR = 0; |
130 | TWDR = 0; |
182 | TWSR = 0; |
131 | TWSR = 0; |
183 | TWBR = 0; |
132 | TWBR = 0; |
184 | I2C_init(); |
133 | I2C_init(); |
185 | I2C_Start(TWI_STATE_MOTOR_TX); |
134 | I2C_Start(TWI_STATE_MOTOR_TX); |
186 | } |
135 | } |
187 | 136 | ||
188 | /**************************************** |
137 | /**************************************** |
189 | * I2C ISR |
138 | * I2C ISR |
190 | ****************************************/ |
139 | ****************************************/ |
191 | ISR (TWI_vect) |
140 | ISR (TWI_vect) |
192 | { |
141 | { |
193 | static uint8_t missing_motor = 0; |
142 | static uint8_t missing_motor = 0; |
194 | switch (twi_state++) { // First i2c_start from SendMotorData() |
143 | switch (twi_state++) { // First i2c_start from SendMotorData() |
195 | // Master Transmit |
144 | // Master Transmit |
196 | case 0: // TWI_STATE_MOTOR_TX |
145 | case 0: // TWI_STATE_MOTOR_TX |
197 | // skip motor if not used in mixer |
146 | // skip motor if not used in mixer |
198 | while ((mixerMatrix.motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
147 | while ((mixerMatrix.motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
199 | motor_write++; |
148 | motor_write++; |
200 | if (motor_write >= MAX_MOTORS) { // writing finished, read now |
149 | if (motor_write >= MAX_MOTORS) { // writing finished, read now |
201 | motor_write = 0; |
150 | motor_write = 0; |
202 | twi_state = TWI_STATE_MOTOR_RX; |
151 | twi_state = TWI_STATE_MOTOR_RX; |
203 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
152 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
204 | } else |
153 | } else |
205 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
154 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
206 | break; |
155 | break; |
207 | case 1: // Send Data to Slave |
156 | case 1: // Send Data to Slave |
208 | I2C_WriteByte(motor[motor_write].throttle); // transmit throttle value. |
157 | I2C_WriteByte(motor[motor_write].throttle); // transmit throttle value. |
209 | break; |
158 | break; |
210 | case 2: // repeat case 0+1 for all motors |
159 | case 2: // repeat case 0+1 for all motors |
211 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
160 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
212 | if (!missing_motor) |
161 | if (!missing_motor) |
213 | missing_motor = motor_write + 1; |
162 | missing_motor = motor_write + 1; |
214 | if (++motor[motor_write].error == 0) |
163 | if (++motor[motor_write].error == 0) |
215 | motor[motor_write].error = 255; // increment error counter and handle overflow |
164 | motor[motor_write].error = 255; // increment error counter and handle overflow |
216 | } |
165 | } |
217 | I2C_Stop(TWI_STATE_MOTOR_TX); |
166 | I2C_Stop(TWI_STATE_MOTOR_TX); |
218 | I2CTimeout = 10; |
167 | I2CTimeout = 10; |
219 | motor_write++; // next motor |
168 | motor_write++; // next motor |
220 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
169 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
221 | break; |
170 | break; |
222 | // Master Receive Data |
171 | // Master Receive Data |
223 | case 3: |
172 | case 3: |
224 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
173 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
225 | // no response from the addressed slave received |
174 | // no response from the addressed slave received |
226 | motor[motor_read].present = 0; |
175 | motor[motor_read].present = 0; |
227 | motor_read++; // next motor |
176 | motor_read++; // next motor |
228 | if (motor_read >= MAX_MOTORS) |
177 | if (motor_read >= MAX_MOTORS) |
229 | motor_read = 0; // restart reading of first motor if we have reached the last one |
178 | motor_read = 0; // restart reading of first motor if we have reached the last one |
230 | I2C_Stop(TWI_STATE_MOTOR_TX); |
179 | I2C_Stop(TWI_STATE_MOTOR_TX); |
231 | } else { |
180 | } else { |
232 | motor[motor_read].present = ('1' - '-') + motor_read; |
181 | motor[motor_read].present = ('1' - '-') + motor_read; |
233 | I2C_ReceiveByte(); //Transmit 1st byte |
182 | I2C_ReceiveByte(); //Transmit 1st byte |
234 | } |
183 | } |
235 | missingMotor = missing_motor; |
184 | missingMotor = missing_motor; |
236 | missing_motor = 0; |
185 | missing_motor = 0; |
237 | break; |
186 | break; |
238 | case 4: //Read 1st byte and transmit 2nd Byte |
187 | case 4: //Read 1st byte and transmit 2nd Byte |
239 | motor[motor_read].current = TWDR; |
188 | motor[motor_read].current = TWDR; |
240 | I2C_ReceiveLastByte(); // nack |
189 | I2C_ReceiveLastByte(); // nack |
241 | break; |
190 | break; |
242 | case 5: |
191 | case 5: |
243 | //Read 2nd byte |
192 | //Read 2nd byte |
244 | motor[motor_read].maxPWM = TWDR; |
193 | motor[motor_read].maxPWM = TWDR; |
245 | motor_read++; // next motor |
194 | motor_read++; // next motor |
246 | if (motor_read >= MAX_MOTORS) |
195 | if (motor_read >= MAX_MOTORS) |
247 | motor_read = 0; // restart reading of first motor if we have reached the last one |
196 | motor_read = 0; // restart reading of first motor if we have reached the last one |
248 | I2C_Stop(TWI_STATE_MOTOR_TX); |
197 | I2C_Stop(TWI_STATE_MOTOR_TX); |
249 | break; |
198 | break; |
250 | 199 | ||
251 | // Writing ADC values. |
200 | // Writing ADC values. |
252 | case 7: |
201 | case 7: |
253 | I2C_WriteByte(0x98); // Address the DAC |
202 | I2C_WriteByte(0x98); // Address the DAC |
254 | break; |
203 | break; |
255 | 204 | ||
256 | case 8: |
205 | case 8: |
257 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
206 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
258 | break; |
207 | break; |
259 | 208 | ||
260 | case 9: |
209 | case 9: |
261 | I2C_WriteByte(gyroAmplifierOffset.offsets[DACChannel]); |
210 | I2C_WriteByte(gyroAmplifierOffset.offsets[DACChannel]); |
262 | break; |
211 | break; |
263 | 212 | ||
264 | case 10: |
213 | case 10: |
265 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
214 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
266 | break; |
215 | break; |
267 | 216 | ||
268 | case 11: |
217 | case 11: |
269 | I2C_Stop(TWI_STATE_MOTOR_TX); |
218 | I2C_Stop(TWI_STATE_MOTOR_TX); |
270 | I2CTimeout = 10; |
219 | I2CTimeout = 10; |
271 | // repeat case 7...10 until all DAC Channels are updated |
220 | // repeat case 7...10 until all DAC Channels are updated |
272 | if (DACChannel < 2) { |
221 | if (DACChannel < 2) { |
273 | DACChannel++; // jump to next channel |
222 | DACChannel++; // jump to next channel |
274 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
223 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
275 | } else { |
224 | } else { |
276 | DACChannel = 0; // reset dac channel counter |
225 | DACChannel = 0; // reset dac channel counter |
277 | } |
226 | } |
278 | break; |
227 | break; |
279 | 228 | ||
280 | default: |
229 | default: |
281 | I2C_Stop(TWI_STATE_MOTOR_TX); |
230 | I2C_Stop(TWI_STATE_MOTOR_TX); |
282 | I2CTimeout = 10; |
231 | I2CTimeout = 10; |
283 | motor_write = 0; |
232 | motor_write = 0; |
284 | motor_read = 0; |
233 | motor_read = 0; |
285 | } |
234 | } |
286 | } |
235 | } |
287 | 236 | ||
288 | extern void twi_diagnostics(void) { |
237 | extern void twi_diagnostics(void) { |
289 | // Check connected BL-Ctrls |
238 | // Check connected BL-Ctrls |
290 | uint8_t i; |
239 | uint8_t i; |
291 | 240 | ||
292 | printf("\n\rFound BL-Ctrl: "); |
241 | printf("\n\rFound BL-Ctrl: "); |
293 | 242 | ||
294 | for (i = 0; i < MAX_MOTORS; i++) { |
243 | for (i = 0; i < MAX_MOTORS; i++) { |
295 | motor[i].throttle = 0; |
244 | motor[i].throttle = 0; |
296 | } |
245 | } |
297 | 246 | ||
298 | I2C_Start(TWI_STATE_MOTOR_TX); |
247 | I2C_Start(TWI_STATE_MOTOR_TX); |
299 | _delay_ms(2); |
248 | _delay_ms(2); |
300 | 249 | ||
301 | motor_read = 0; // read the first I2C-Data |
250 | motor_read = 0; // read the first I2C-Data |
302 | 251 | ||
303 | for (i = 0; i < MAX_MOTORS; i++) { |
252 | for (i = 0; i < MAX_MOTORS; i++) { |
304 | I2C_Start(TWI_STATE_MOTOR_TX); |
253 | I2C_Start(TWI_STATE_MOTOR_TX); |
305 | _delay_ms(2); |
254 | _delay_ms(2); |
306 | if (motor[i].present) |
255 | if (motor[i].present) |
307 | printf("%d ",i+1); |
256 | printf("%d ",i+1); |
308 | } |
257 | } |
309 | 258 | ||
310 | for (i = 0; i < MAX_MOTORS; i++) { |
259 | for (i = 0; i < MAX_MOTORS; i++) { |
311 | if (!motor[i].present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
260 | if (!motor[i].present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
312 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
261 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
313 | motor[i].error = 0; |
262 | motor[i].error = 0; |
314 | } |
263 | } |
315 | } |
264 | } |
316 | 265 |