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