Rev 1775 | Rev 1868 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1775 | Rev 1821 | ||
---|---|---|---|
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 excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
35 | // + Commercial use (for excample: 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 "eeprom.h" |
56 | //#include "eeprom.h" |
57 | #include "twimaster.h" |
57 | #include "twimaster.h" |
58 | //#include "analog.h" |
58 | //#include "analog.h" |
59 | #include "configuration.h" |
59 | #include "configuration.h" |
60 | #include "printf_P.h" |
60 | #include "printf_P.h" |
61 | 61 | ||
62 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
62 | volatile uint8_t twi_state = TWI_STATE_MOTOR_TX; |
63 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t dac_channel = 0; |
64 | volatile uint8_t motor_write = 0; |
64 | volatile uint8_t motor_write = 0; |
65 | volatile uint8_t motor_read = 0; |
65 | volatile uint8_t motor_read = 0; |
66 | volatile uint16_t I2CTimeout = 100; |
66 | volatile uint16_t I2CTimeout = 100; |
67 | uint8_t missingMotor = 0; |
67 | uint8_t missingMotor = 0; |
68 | 68 | ||
69 | MotorData_t motor[MAX_MOTORS]; |
69 | MotorData_t motor[MAX_MOTORS]; |
70 | 70 | ||
71 | volatile uint8_t DACValues[4]; |
71 | volatile uint8_t DACValues[4]; |
72 | uint8_t DACChannel = 0; |
72 | uint8_t DACChannel = 0; |
73 | 73 | ||
74 | #define SCL_CLOCK 200000L |
74 | #define SCL_CLOCK 200000L |
75 | #define I2C_TIMEOUT 30000 |
75 | #define I2C_TIMEOUT 30000 |
76 | 76 | ||
77 | /************************************************** |
77 | /************************************************** |
78 | * Initialize I2C (TWI) |
78 | * Initialize I2C (TWI) |
79 | **************************************************/ |
79 | **************************************************/ |
80 | void I2C_init(void) { |
80 | void I2C_init(void) { |
81 | uint8_t i; |
81 | uint8_t i; |
82 | uint8_t sreg = SREG; |
82 | uint8_t sreg = SREG; |
83 | cli(); |
83 | cli(); |
84 | 84 | ||
85 | // SDA is INPUT |
85 | // SDA is INPUT |
86 | DDRC &= ~(1<<DDC1); |
86 | DDRC &= ~(1 << DDC1); |
87 | // SCL is output |
87 | // SCL is output |
88 | DDRC |= (1<<DDC0); |
88 | DDRC |= (1 << DDC0); |
89 | // pull up SDA |
89 | // pull up SDA |
90 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
90 | PORTC |= (1 << PORTC0) | (1 << PORTC1); |
91 | 91 | ||
92 | // TWI Status Register |
92 | // TWI Status Register |
93 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
93 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
94 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
94 | TWSR &= ~((1 << TWPS1) | (1 << TWPS0)); |
95 | 95 | ||
96 | // set TWI Bit Rate Register |
96 | // set TWI Bit Rate Register |
97 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
97 | TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2; |
98 | 98 | ||
99 | twi_state = TWI_STATE_MOTOR_TX; |
99 | twi_state = TWI_STATE_MOTOR_TX; |
100 | motor_write = 0; |
100 | motor_write = 0; |
101 | motor_read = 0; |
101 | motor_read = 0; |
102 | 102 | ||
103 | for(i=0; i < MAX_MOTORS; i++) { |
103 | for (i = 0; i < MAX_MOTORS; i++) { |
104 | motor[i].SetPoint = 0; |
104 | motor[i].SetPoint = 0; |
105 | motor[i].Present = 0; |
105 | motor[i].Present = 0; |
106 | motor[i].Error = 0; |
106 | motor[i].Error = 0; |
107 | motor[i].MaxPWM = 0; |
107 | motor[i].MaxPWM = 0; |
108 | } |
108 | } |
109 | 109 | ||
110 | SREG = sreg; |
110 | SREG = sreg; |
111 | } |
111 | } |
112 | 112 | ||
113 | /**************************************** |
113 | /**************************************** |
114 | * Start I2C |
114 | * Start I2C |
115 | ****************************************/ |
115 | ****************************************/ |
116 | void I2C_Start(uint8_t start_state) { |
116 | void I2C_Start(uint8_t start_state) { |
117 | twi_state = start_state; |
117 | twi_state = start_state; |
118 | // TWI Control Register |
118 | // TWI Control Register |
119 | // clear TWI interrupt flag (TWINT=1) |
119 | // clear TWI interrupt flag (TWINT=1) |
120 | // disable TWI Acknowledge Bit (TWEA = 0) |
120 | // disable TWI Acknowledge Bit (TWEA = 0) |
121 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
121 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
122 | // disable TWI STOP Condition Bit (TWSTO = 0) |
122 | // disable TWI STOP Condition Bit (TWSTO = 0) |
123 | // disable TWI Write Collision Flag (TWWC = 0) |
123 | // disable TWI Write Collision Flag (TWWC = 0) |
124 | // enable i2c (TWEN = 1) |
124 | // enable i2c (TWEN = 1) |
125 | // enable TWI Interrupt (TWIE = 1) |
125 | // enable TWI Interrupt (TWIE = 1) |
126 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
126 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
127 | } |
127 | } |
128 | 128 | ||
129 | /**************************************** |
129 | /**************************************** |
130 | * Stop I2C |
130 | * Stop I2C |
131 | ****************************************/ |
131 | ****************************************/ |
132 | void I2C_Stop(uint8_t start_state) { |
132 | void I2C_Stop(uint8_t start_state) { |
133 | twi_state = start_state; |
133 | twi_state = start_state; |
134 | // TWI Control Register |
134 | // TWI Control Register |
135 | // clear TWI interrupt flag (TWINT=1) |
135 | // clear TWI interrupt flag (TWINT=1) |
136 | // disable TWI Acknowledge Bit (TWEA = 0) |
136 | // disable TWI Acknowledge Bit (TWEA = 0) |
137 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
137 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
138 | // enable TWI STOP Condition Bit (TWSTO = 1) |
138 | // enable TWI STOP Condition Bit (TWSTO = 1) |
139 | // disable TWI Write Collision Flag (TWWC = 0) |
139 | // disable TWI Write Collision Flag (TWWC = 0) |
140 | // enable i2c (TWEN = 1) |
140 | // enable i2c (TWEN = 1) |
141 | // disable TWI Interrupt (TWIE = 0) |
141 | // disable TWI Interrupt (TWIE = 0) |
142 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
142 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
143 | } |
143 | } |
144 | 144 | ||
145 | /**************************************** |
145 | /**************************************** |
146 | * Write to I2C |
146 | * Write to I2C |
147 | ****************************************/ |
147 | ****************************************/ |
148 | void I2C_WriteByte(int8_t byte) { |
148 | void I2C_WriteByte(int8_t byte) { |
149 | // move byte to send into TWI Data Register |
149 | // move byte to send into TWI Data Register |
150 | TWDR = byte; |
150 | TWDR = byte; |
151 | // clear interrupt flag (TWINT = 1) |
151 | // clear interrupt flag (TWINT = 1) |
152 | // enable i2c bus (TWEN = 1) |
152 | // enable i2c bus (TWEN = 1) |
153 | // enable interrupt (TWIE = 1) |
153 | // enable interrupt (TWIE = 1) |
154 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
154 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
155 | } |
155 | } |
156 | 156 | ||
157 | /**************************************** |
157 | /**************************************** |
158 | * Receive byte and send ACK |
158 | * Receive byte and send ACK |
159 | ****************************************/ |
159 | ****************************************/ |
160 | void I2C_ReceiveByte(void) { |
160 | void I2C_ReceiveByte(void) { |
161 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
161 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
162 | } |
162 | } |
163 | 163 | ||
164 | /**************************************** |
164 | /**************************************** |
165 | * I2C receive last byte and send no ACK |
165 | * I2C receive last byte and send no ACK |
166 | ****************************************/ |
166 | ****************************************/ |
167 | void I2C_ReceiveLastByte(void){ |
167 | void I2C_ReceiveLastByte(void) { |
168 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
168 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
169 | } |
169 | } |
170 | 170 | ||
171 | /**************************************** |
171 | /**************************************** |
172 | * Reset I2C |
172 | * Reset I2C |
173 | ****************************************/ |
173 | ****************************************/ |
174 | void I2C_Reset(void) { |
174 | void I2C_Reset(void) { |
175 | // stop i2c bus |
175 | // stop i2c bus |
176 | I2C_Stop(TWI_STATE_MOTOR_TX); |
176 | I2C_Stop(TWI_STATE_MOTOR_TX); |
177 | twi_state = 0; |
177 | twi_state = 0; |
178 | motor_write = TWDR; |
178 | motor_write = TWDR; |
179 | motor_write = 0; |
179 | motor_write = 0; |
180 | motor_read = 0; |
180 | motor_read = 0; |
181 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
181 | TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
182 | TWAMR = 0; |
182 | TWAMR = 0; |
183 | TWAR = 0; |
183 | TWAR = 0; |
184 | TWDR = 0; |
184 | TWDR = 0; |
185 | TWSR = 0; |
185 | TWSR = 0; |
186 | TWBR = 0; |
186 | TWBR = 0; |
187 | I2C_init(); |
187 | I2C_init(); |
188 | I2C_Start(TWI_STATE_MOTOR_TX); |
188 | I2C_Start(TWI_STATE_MOTOR_TX); |
189 | } |
189 | } |
190 | 190 | ||
191 | /**************************************** |
191 | /**************************************** |
192 | * I2C ISR |
192 | * I2C ISR |
193 | ****************************************/ |
193 | ****************************************/ |
194 | ISR (TWI_vect) { |
194 | ISR (TWI_vect) |
- | 195 | { |
|
195 | static uint8_t missing_motor = 0; |
196 | static uint8_t missing_motor = 0; |
196 | switch (twi_state++) { // First i2c_start from SendMotorData() |
197 | switch (twi_state++) { // First i2c_start from SendMotorData() |
197 | // Master Transmit |
198 | // Master Transmit |
198 | case 0: // TWI_STATE_MOTOR_TX |
199 | case 0: // TWI_STATE_MOTOR_TX |
199 | // skip motor if not used in mixer |
200 | // skip motor if not used in mixer |
200 | while((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) motor_write++; |
201 | while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write |
- | 202 | < MAX_MOTORS)) |
|
- | 203 | motor_write++; |
|
201 | if(motor_write >= MAX_MOTORS) { // writing finished, read now |
204 | if (motor_write >= MAX_MOTORS) { // writing finished, read now |
202 | motor_write = 0; |
205 | motor_write = 0; |
203 | twi_state = TWI_STATE_MOTOR_RX; |
206 | twi_state = TWI_STATE_MOTOR_RX; |
204 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
207 | I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode |
205 | } |
208 | } else |
206 | else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
209 | I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
207 | break; |
210 | break; |
208 | case 1: // Send Data to Slave |
211 | case 1: // Send Data to Slave |
209 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
212 | I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
210 | break; |
213 | break; |
211 | case 2: // repeat case 0+1 for all motors |
214 | case 2: // repeat case 0+1 for all motors |
212 | if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
215 | if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
- | 216 | if (!missing_motor) |
|
213 | if(!missing_motor) missing_motor = motor_write + 1; |
217 | missing_motor = motor_write + 1; |
- | 218 | if (++motor[motor_write].Error == 0) |
|
214 | if(++motor[motor_write].Error == 0) motor[motor_write].Error = 255; // increment error counter and handle overflow |
219 | motor[motor_write].Error = 255; // increment error counter and handle overflow |
215 | } |
220 | } |
216 | I2C_Stop(TWI_STATE_MOTOR_TX); |
221 | I2C_Stop(TWI_STATE_MOTOR_TX); |
217 | I2CTimeout = 10; |
222 | I2CTimeout = 10; |
218 | motor_write++; // next motor |
223 | motor_write++; // next motor |
219 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
224 | I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
220 | break; |
225 | break; |
221 | // Master Receive Data |
226 | // Master Receive Data |
222 | case 3: |
227 | case 3: |
223 | if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
228 | if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
224 | // no response from the addressed slave received |
229 | // no response from the addressed slave received |
225 | motor[motor_read].Present = 0; |
230 | motor[motor_read].Present = 0; |
226 | motor_read++; // next motor |
231 | motor_read++; // next motor |
- | 232 | if (motor_read >= MAX_MOTORS) |
|
227 | if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
233 | motor_read = 0; // restart reading of first motor if we have reached the last one |
228 | I2C_Stop(TWI_STATE_MOTOR_TX); |
234 | I2C_Stop(TWI_STATE_MOTOR_TX); |
229 | } else { |
235 | } else { |
230 | motor[motor_read].Present = ('1' - '-') + motor_read; |
236 | motor[motor_read].Present = ('1' - '-') + motor_read; |
231 | I2C_ReceiveByte(); //Transmit 1st byte |
237 | I2C_ReceiveByte(); //Transmit 1st byte |
232 | } |
238 | } |
233 | missingMotor = missing_motor; |
239 | missingMotor = missing_motor; |
234 | missing_motor = 0; |
240 | missing_motor = 0; |
235 | break; |
241 | break; |
236 | case 4: //Read 1st byte and transmit 2nd Byte |
242 | case 4: //Read 1st byte and transmit 2nd Byte |
237 | motor[motor_read].Current = TWDR; |
243 | motor[motor_read].Current = TWDR; |
238 | I2C_ReceiveLastByte(); // nack |
244 | I2C_ReceiveLastByte(); // nack |
239 | break; |
245 | break; |
240 | case 5: |
246 | case 5: |
241 | //Read 2nd byte |
247 | //Read 2nd byte |
242 | motor[motor_read].MaxPWM = TWDR; |
248 | motor[motor_read].MaxPWM = TWDR; |
243 | motor_read++; // next motor |
249 | motor_read++; // next motor |
- | 250 | if (motor_read >= MAX_MOTORS) |
|
244 | if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one |
251 | motor_read = 0; // restart reading of first motor if we have reached the last one |
245 | I2C_Stop(TWI_STATE_MOTOR_TX); |
252 | I2C_Stop(TWI_STATE_MOTOR_TX); |
246 | break; |
253 | break; |
247 | 254 | ||
248 | // Writing ADC values. |
255 | // Writing ADC values. |
249 | case 7: |
256 | case 7: |
250 | I2C_WriteByte(0x98); // Address the DAC |
257 | I2C_WriteByte(0x98); // Address the DAC |
251 | break; |
258 | break; |
252 | 259 | ||
253 | case 8: |
260 | case 8: |
254 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
261 | I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
255 | break; |
262 | break; |
256 | 263 | ||
257 | case 9: |
264 | case 9: |
258 | I2C_WriteByte(DACValues[DACChannel]); |
265 | I2C_WriteByte(DACValues[DACChannel]); |
259 | break; |
266 | break; |
260 | 267 | ||
261 | case 10: |
268 | case 10: |
262 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
269 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
263 | break; |
270 | break; |
264 | 271 | ||
265 | case 11: |
272 | case 11: |
266 | I2C_Stop(TWI_STATE_MOTOR_TX); |
273 | I2C_Stop(TWI_STATE_MOTOR_TX); |
267 | I2CTimeout = 10; |
274 | I2CTimeout = 10; |
268 | // repeat case 7...10 until all DAC Channels are updated |
275 | // repeat case 7...10 until all DAC Channels are updated |
269 | if(DACChannel < 2) { |
276 | if (DACChannel < 2) { |
270 | DACChannel ++; // jump to next channel |
277 | DACChannel++; // jump to next channel |
271 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
278 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
272 | } else { |
279 | } else { |
273 | DACChannel = 0; // reset dac channel counter |
280 | DACChannel = 0; // reset dac channel counter |
274 | } |
281 | } |
275 | break; |
282 | break; |
276 | 283 | ||
277 | default: |
284 | default: |
278 | I2C_Stop(TWI_STATE_MOTOR_TX); |
285 | I2C_Stop(TWI_STATE_MOTOR_TX); |
279 | I2CTimeout = 10; |
286 | I2CTimeout = 10; |
280 | motor_write = 0; |
287 | motor_write = 0; |
281 | motor_read = 0; |
288 | motor_read = 0; |
282 | } |
289 | } |
283 | } |
290 | } |
284 | 291 | ||
285 | extern void twi_diagnostics(void) { |
292 | extern void twi_diagnostics(void) { |
286 | // Check connected BL-Ctrls |
293 | // Check connected BL-Ctrls |
287 | uint8_t i; |
294 | uint8_t i; |
288 | 295 | ||
289 | printf("\n\rFound BL-Ctrl: "); |
296 | printf("\n\rFound BL-Ctrl: "); |
290 | 297 | ||
291 | for(i = 0; i < MAX_MOTORS; i++) { |
298 | for (i = 0; i < MAX_MOTORS; i++) { |
292 | motor[i].SetPoint = 0; |
299 | motor[i].SetPoint = 0; |
293 | } |
300 | } |
294 | 301 | ||
295 | I2C_Start(TWI_STATE_MOTOR_TX); |
302 | I2C_Start(TWI_STATE_MOTOR_TX); |
296 | _delay_ms(2); |
303 | _delay_ms(2); |
297 | 304 | ||
298 | motor_read = 0; // read the first I2C-Data |
305 | motor_read = 0; // read the first I2C-Data |
299 | 306 | ||
300 | for(i = 0; i < MAX_MOTORS; i++) { |
307 | for (i = 0; i < MAX_MOTORS; i++) { |
301 | I2C_Start(TWI_STATE_MOTOR_TX); |
308 | I2C_Start(TWI_STATE_MOTOR_TX); |
302 | _delay_ms(2); |
309 | _delay_ms(2); |
303 | if(motor[i].Present) printf("%d ",i+1); |
310 | if (motor[i].Present) |
- | 311 | printf("%d ",i+1); |
|
304 | } |
312 | } |
305 | 313 | ||
306 | for(i = 0; i < MAX_MOTORS; i++) { |
314 | for (i = 0; i < MAX_MOTORS; i++) { |
307 | if(!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
315 | if (!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) |
- | 316 | printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
|
308 | motor[i].Error = 0; |
317 | motor[i].Error = 0; |
309 | } |
318 | } |
310 | } |
319 | } |
311 | 320 |