Rev 1179 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1179 | Rev 1180 | ||
|---|---|---|---|
| - | 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 excample: 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 |
|
| 1 | /*############################################################################ |
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 |
|
| 2 | ############################################################################*/ |
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 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 3 | 51 | ||
| 4 | #include <avr/io.h> |
52 | #include <avr/io.h> |
| 5 | #include <avr/interrupt.h> |
53 | #include <avr/interrupt.h> |
| 6 | 54 | ||
| 7 | #include "main.h" |
55 | #include "main.h" |
| 8 | #include "twimaster.h" |
56 | #include "twimaster.h" |
| 9 | #include "fc.h" |
57 | #include "fc.h" |
| 10 | #include "analog.h" |
58 | #include "analog.h" |
| 11 | 59 | ||
| 12 | volatile uint8_t twi_state = 0; |
60 | volatile uint8_t twi_state = 0; |
| 13 | volatile uint8_t motor_write = 0; |
61 | uint8_t motor_write = 0; |
| 14 | volatile uint8_t motor_read = 0; |
62 | uint8_t motor_read = 0; |
| 15 | volatile uint8_t dac_channel = 0; |
63 | volatile uint8_t dac_channel = 0; |
| - | 64 | ||
| - | 65 | #ifdef USE_QUADRO |
|
| 16 | volatile uint8_t motor_rx[8]; |
66 | uint8_t motor_rx[8]; |
| - | 67 | #else |
|
| - | 68 | uint8_t motor_rx[16]; |
|
| - | 69 | #endif |
|
| - | 70 | ||
| 17 | volatile uint16_t I2CTimeout = 100; |
71 | volatile uint16_t I2CTimeout = 100; |
| 18 | 72 | ||
| 19 | 73 | ||
| 20 | #define SCL_CLOCK 200000L |
74 | #define SCL_CLOCK 200000L |
| 21 | #define I2C_TIMEOUT 30000 |
75 | #define I2C_TIMEOUT 30000 |
| 22 | 76 | ||
| 23 | #define TWSR_STATUS_MASK 0xF8 |
77 | #define TWSR_STATUS_MASK 0xF8 |
| 24 | // for Master Transmitter Mode |
78 | // for Master Transmitter Mode |
| 25 | 79 | ||
| 26 | #define I2C_STATUS_START 0x08 |
80 | #define I2C_STATUS_START 0x08 |
| 27 | #define I2C_STATUS_REPEATSTART 0x10 |
81 | #define I2C_STATUS_REPEATSTART 0x10 |
| 28 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
82 | #define I2C_STATUS_TX_SLA_ACK 0x18 |
| 29 | #define I2C_STATUS_SLAW_NOACK 0x20 |
83 | #define I2C_STATUS_SLAW_NOACK 0x20 |
| 30 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
84 | #define I2C_STATUS_TX_DATA_ACK 0x28 |
| 31 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
85 | #define I2C_STATUS_TX_DATA_NOTACK 0x30 |
| 32 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
86 | #define I2C_STATUS_RX_DATA_ACK 0x50 |
| 33 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
87 | #define I2C_STATUS_RX_DATA_NOTACK 0x58 |
| 34 | 88 | ||
| 35 | /**************************************************/ |
89 | /**************************************************/ |
| 36 | /* Initialize I2C (TWI) */ |
90 | /* Initialize I2C (TWI) */ |
| 37 | /**************************************************/ |
91 | /**************************************************/ |
| 38 | void I2C_Init(void) |
92 | void I2C_Init(void) |
| 39 | { |
93 | { |
| 40 | uint8_t sreg = SREG; |
94 | uint8_t sreg = SREG; |
| 41 | cli(); |
95 | cli(); |
| 42 | 96 | ||
| 43 | // SDA is INPUT |
97 | // SDA is INPUT |
| 44 | DDRC &= ~(1<<DDC1); |
98 | DDRC &= ~(1<<DDC1); |
| 45 | // SCL is output |
99 | // SCL is output |
| 46 | DDRC |= (1<<DDC0); |
100 | DDRC |= (1<<DDC0); |
| 47 | // pull up SDA |
101 | // pull up SDA |
| 48 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
102 | PORTC |= (1<<PORTC0)|(1<<PORTC1); |
| 49 | 103 | ||
| 50 | // TWI Status Register |
104 | // TWI Status Register |
| 51 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
105 | // prescaler 1 (TWPS1 = 0, TWPS0 = 0) |
| 52 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
106 | TWSR &= ~((1<<TWPS1)|(1<<TWPS0)); |
| 53 | 107 | ||
| 54 | // set TWI Bit Rate Register |
108 | // set TWI Bit Rate Register |
| 55 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
109 | TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
| 56 | 110 | ||
| 57 | twi_state = 0; |
111 | twi_state = 0; |
| 58 | motor_write = 0; |
112 | motor_write = 0; |
| 59 | motor_read = 0; |
113 | motor_read = 0; |
| 60 | 114 | ||
| 61 | SREG = sreg; |
115 | SREG = sreg; |
| 62 | } |
116 | } |
| 63 | 117 | ||
| 64 | /****************************************/ |
118 | /****************************************/ |
| 65 | /* Start I2C */ |
119 | /* Start I2C */ |
| 66 | /****************************************/ |
120 | /****************************************/ |
| 67 | void I2C_Start(void) |
121 | void I2C_Start(void) |
| 68 | { |
122 | { |
| 69 | // TWI Control Register |
123 | // TWI Control Register |
| 70 | // clear TWI interrupt flag (TWINT=1) |
124 | // clear TWI interrupt flag (TWINT=1) |
| 71 | // disable TWI Acknowledge Bit (TWEA = 0) |
125 | // disable TWI Acknowledge Bit (TWEA = 0) |
| 72 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
126 | // enable TWI START Condition Bit (TWSTA = 1), MASTER |
| 73 | // disable TWI STOP Condition Bit (TWSTO = 0) |
127 | // disable TWI STOP Condition Bit (TWSTO = 0) |
| 74 | // disable TWI Write Collision Flag (TWWC = 0) |
128 | // disable TWI Write Collision Flag (TWWC = 0) |
| 75 | // enable i2c (TWEN = 1) |
129 | // enable i2c (TWEN = 1) |
| 76 | // enable TWI Interrupt (TWIE = 1) |
130 | // enable TWI Interrupt (TWIE = 1) |
| 77 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
131 | TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE); |
| 78 | } |
132 | } |
| 79 | 133 | ||
| 80 | /****************************************/ |
134 | /****************************************/ |
| 81 | /* Stop I2C */ |
135 | /* Stop I2C */ |
| 82 | /****************************************/ |
136 | /****************************************/ |
| 83 | void I2C_Stop(void) |
137 | void I2C_Stop(void) |
| 84 | { |
138 | { |
| 85 | // TWI Control Register |
139 | // TWI Control Register |
| 86 | // clear TWI interrupt flag (TWINT=1) |
140 | // clear TWI interrupt flag (TWINT=1) |
| 87 | // disable TWI Acknowledge Bit (TWEA = 0) |
141 | // disable TWI Acknowledge Bit (TWEA = 0) |
| 88 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
142 | // diable TWI START Condition Bit (TWSTA = 1), no MASTER |
| 89 | // enable TWI STOP Condition Bit (TWSTO = 1) |
143 | // enable TWI STOP Condition Bit (TWSTO = 1) |
| 90 | // disable TWI Write Collision Flag (TWWC = 0) |
144 | // disable TWI Write Collision Flag (TWWC = 0) |
| 91 | // enable i2c (TWEN = 1) |
145 | // enable i2c (TWEN = 1) |
| 92 | // disable TWI Interrupt (TWIE = 0) |
146 | // disable TWI Interrupt (TWIE = 0) |
| 93 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
147 | TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN); |
| 94 | } |
148 | } |
| 95 | 149 | ||
| 96 | 150 | ||
| 97 | /****************************************/ |
151 | /****************************************/ |
| 98 | /* Write to I2C */ |
152 | /* Write to I2C */ |
| 99 | /****************************************/ |
153 | /****************************************/ |
| 100 | void I2C_WriteByte(int8_t byte) |
154 | void I2C_WriteByte(int8_t byte) |
| 101 | { |
155 | { |
| 102 | // move byte to send into TWI Data Register |
156 | // move byte to send into TWI Data Register |
| 103 | TWDR = byte; |
157 | TWDR = byte; |
| 104 | // clear interrupt flag (TWINT = 1) |
158 | // clear interrupt flag (TWINT = 1) |
| 105 | // enable i2c bus (TWEN = 1) |
159 | // enable i2c bus (TWEN = 1) |
| 106 | // enable interrupt (TWIE = 1) |
160 | // enable interrupt (TWIE = 1) |
| 107 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
161 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| 108 | } |
162 | } |
| 109 | 163 | ||
| 110 | 164 | ||
| 111 | /****************************************/ |
165 | /****************************************/ |
| 112 | /* Receive byte and send ACK */ |
166 | /* Receive byte and send ACK */ |
| 113 | /****************************************/ |
167 | /****************************************/ |
| 114 | void I2C_ReceiveByte(void) |
168 | void I2C_ReceiveByte(void) |
| 115 | { |
169 | { |
| 116 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
170 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
| 117 | } |
171 | } |
| 118 | 172 | ||
| 119 | /****************************************/ |
173 | /****************************************/ |
| 120 | /* I2C receive last byte and send no ACK*/ |
174 | /* I2C receive last byte and send no ACK*/ |
| 121 | /****************************************/ |
175 | /****************************************/ |
| 122 | void I2C_ReceiveLastByte(void) |
176 | void I2C_ReceiveLastByte(void) |
| 123 | { |
177 | { |
| 124 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
178 | TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
| 125 | } |
179 | } |
| 126 | 180 | ||
| 127 | 181 | ||
| 128 | /****************************************/ |
182 | /****************************************/ |
| 129 | /* Reset I2C */ |
183 | /* Reset I2C */ |
| 130 | /****************************************/ |
184 | /****************************************/ |
| 131 | void I2C_Reset(void) |
185 | void I2C_Reset(void) |
| 132 | { |
186 | { |
| 133 | // stop i2c bus |
187 | // stop i2c bus |
| 134 | I2C_Stop(); |
188 | I2C_Stop(); |
| 135 | twi_state = 0; |
189 | twi_state = 0; |
| 136 | motor_write = TWDR; |
190 | motor_write = TWDR; |
| 137 | motor_write = 0; |
191 | motor_write = 0; |
| 138 | motor_read = 0; |
192 | motor_read = 0; |
| 139 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
193 | TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset |
| 140 | TWAMR = 0; |
194 | TWAMR = 0; |
| 141 | TWAR = 0; |
195 | TWAR = 0; |
| 142 | TWDR = 0; |
196 | TWDR = 0; |
| 143 | TWSR = 0; |
197 | TWSR = 0; |
| 144 | TWBR = 0; |
198 | TWBR = 0; |
| 145 | I2C_Init(); |
199 | I2C_Init(); |
| 146 | I2C_Start(); |
200 | I2C_Start(); |
| 147 | I2C_WriteByte(0); |
201 | I2C_WriteByte(0); |
| 148 | } |
202 | } |
| - | 203 | ||
| 149 | 204 | ||
| 150 | /****************************************/ |
205 | /****************************************/ |
| 151 | /* I2C ISR */ |
206 | /* I2C ISR */ |
| 152 | /****************************************/ |
207 | /****************************************/ |
| 153 | ISR (TWI_vect) |
- | |
| - | 208 | ||
| - | 209 | #ifdef USE_QUADRO |
|
| 154 | 210 | ISR (TWI_vect) |
|
| 155 | { |
211 | { |
| 156 | 212 | ||
| 157 | switch (twi_state++) // First i2c_start from SendMotorData() |
213 | switch (twi_state++) // First i2c_start from SendMotorData() |
| 158 | { |
214 | { |
| 159 | // Master Transmit |
215 | // Master Transmit |
| 160 | case 0: // Send SLA-W |
216 | case 0: // Send SLA-W |
| 161 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
217 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
| 162 | break; |
218 | break; |
| 163 | case 1: // Send Data to Slave |
219 | case 1: // Send Data to Slave |
| 164 | switch(motor_write) |
220 | switch(motor_write) |
| 165 | { |
221 | { |
| 166 | case 0: |
222 | case 0: |
| 167 | I2C_WriteByte(Motor_Front); |
223 | I2C_WriteByte(Motor1); |
| 168 | break; |
224 | break; |
| 169 | case 1: |
225 | case 1: |
| 170 | I2C_WriteByte(Motor_Rear); |
226 | I2C_WriteByte(Motor2); |
| 171 | break; |
227 | break; |
| 172 | case 2: |
228 | case 2: |
| 173 | I2C_WriteByte(Motor_Right); |
229 | I2C_WriteByte(Motor3); |
| 174 | break; |
230 | break; |
| 175 | case 3: |
231 | case 3: |
| 176 | I2C_WriteByte(Motor_Left); |
232 | I2C_WriteByte(Motor4); |
| 177 | break; |
233 | break; |
| 178 | } |
234 | } |
| 179 | break; |
235 | break; |
| 180 | case 2: // repeat case 0+1 for all motors |
236 | case 2: // repeat case 0+1 for all motors |
| 181 | I2C_Stop(); |
237 | I2C_Stop(); |
| 182 | if (motor_write < 3) |
238 | if (motor_write < 3) |
| 183 | { |
239 | { |
| 184 | motor_write++; // jump to next motor |
240 | motor_write++; // jump to next motor |
| 185 | twi_state = 0; // and repeat from state 0 |
241 | twi_state = 0; // and repeat from state 0 |
| 186 | } |
242 | } |
| 187 | else |
243 | else |
| 188 | { // data to last motor send |
244 | { // data to last motor send |
| 189 | motor_write = 0; // reset motor write counter |
245 | motor_write = 0; // reset motor write counter |
| 190 | } |
246 | } |
| 191 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
247 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
| 192 | break; |
248 | break; |
| 193 | 249 | ||
| 194 | // Master Receive |
250 | // Master Receive |
| 195 | case 3: // Send SLA-R |
251 | case 3: // Send SLA-R |
| 196 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
252 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
| 197 | break; |
253 | break; |
| 198 | case 4: |
254 | case 4: |
| 199 | //Transmit 1st byte |
255 | //Transmit 1st byte |
| 200 | I2C_ReceiveByte(); |
256 | I2C_ReceiveByte(); |
| 201 | break; |
257 | break; |
| 202 | case 5: //Read 1st byte and transmit 2nd Byte |
258 | case 5: //Read 1st byte and transmit 2nd Byte |
| 203 | motor_rx[motor_read] = TWDR; |
259 | motor_rx[motor_read] = TWDR; |
| 204 | I2C_ReceiveLastByte(); |
260 | I2C_ReceiveLastByte(); |
| 205 | break; |
261 | break; |
| 206 | case 6: |
262 | case 6: |
| 207 | //Read 2nd byte |
263 | //Read 2nd byte |
| 208 | motor_rx[motor_read + 4] = TWDR; |
264 | motor_rx[motor_read + 4] = TWDR; |
| 209 | motor_read++; |
265 | motor_read++; |
| 210 | if (motor_read > 3) motor_read = 0; |
266 | if (motor_read > 3) motor_read = 0; |
| 211 | I2C_Stop(); |
267 | I2C_Stop(); |
| 212 | twi_state = 0; |
268 | twi_state = 0; |
| 213 | I2CTimeout = 10; |
269 | I2CTimeout = 10; |
| 214 | break; |
270 | break; |
| 215 | 271 | ||
| 216 | // Gyro-Offsets |
272 | // Gyro-Offsets |
| 217 | case 7: |
273 | case 7: |
| 218 | I2C_WriteByte(0x98); // Address the DAC |
274 | I2C_WriteByte(0x98); // Address the DAC |
| 219 | break; |
275 | break; |
| 220 | 276 | ||
| 221 | case 8: |
277 | case 8: |
| 222 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
278 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
| 223 | break; |
279 | break; |
| 224 | 280 | ||
| 225 | case 9: |
281 | case 9: |
| 226 | switch(dac_channel) |
282 | switch(dac_channel) |
| 227 | { |
283 | { |
| 228 | case 0: |
284 | case 0: |
| 229 | I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A |
285 | I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
| 230 | break; |
286 | break; |
| 231 | case 1: |
287 | case 1: |
| 232 | I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B |
288 | I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
| 233 | break; |
289 | break; |
| 234 | case 2: |
290 | case 2: |
| 235 | I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C |
291 | I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
| 236 | break; |
292 | break; |
| 237 | } |
293 | } |
| 238 | break; |
294 | break; |
| 239 | 295 | ||
| 240 | case 10: |
296 | case 10: |
| 241 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
297 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
| 242 | break; |
298 | break; |
| 243 | 299 | ||
| 244 | case 11: |
300 | case 11: |
| 245 | I2C_Stop(); |
301 | I2C_Stop(); |
| 246 | I2CTimeout = 10; |
302 | I2CTimeout = 10; |
| 247 | // repeat case 7...10 until all DAC Channels are updated |
303 | // repeat case 7...10 until all DAC Channels are updated |
| 248 | if(dac_channel < 2) |
304 | if(dac_channel < 2) |
| 249 | { |
305 | { |
| 250 | dac_channel ++; // jump to next channel |
306 | dac_channel ++; // jump to next channel |
| 251 | twi_state = 7; // and repeat from state 7 |
307 | twi_state = 7; // and repeat from state 7 |
| 252 | I2C_Start(); // start transmission for next channel |
308 | I2C_Start(); // start transmission for next channel |
| 253 | } |
309 | } |
| 254 | else |
310 | else |
| 255 | { // data to last motor send |
311 | { // data to last motor send |
| 256 | dac_channel = 0; // reset dac channel counter |
312 | dac_channel = 0; // reset dac channel counter |
| 257 | twi_state = 0; // reset twi_state |
313 | twi_state = 0; // reset twi_state |
| 258 | } |
314 | } |
| 259 | break; |
315 | break; |
| 260 | 316 | ||
| 261 | default: |
317 | default: |
| 262 | I2C_Stop(); |
318 | I2C_Stop(); |
| 263 | twi_state = 0; |
319 | twi_state = 0; |
| 264 | I2CTimeout = 10; |
320 | I2CTimeout = 10; |
| 265 | motor_write = 0; |
321 | motor_write = 0; |
| 266 | motor_read = 0; |
322 | motor_read = 0; |
| 267 | } |
323 | } |
| 268 | } |
324 | } |
| - | 325 | #else // USE_OCTO, USE_OCTO2, USE_OCTO3 |
|
| - | 326 | ISR (TWI_vect) |
|
| - | 327 | { |
|
| - | 328 | ||
| - | 329 | switch (twi_state++) // First i2c_start from SendMotorData() |
|
| - | 330 | { |
|
| - | 331 | // Master Transmit |
|
| - | 332 | case 0: // Send SLA-W |
|
| - | 333 | I2C_WriteByte(0x52 + (motor_write * 2) ); |
|
| - | 334 | break; |
|
| - | 335 | case 1: // Send Data to Slave |
|
| - | 336 | switch(motor_write) |
|
| - | 337 | { |
|
| - | 338 | case 0: |
|
| - | 339 | I2C_WriteByte(Motor1); |
|
| - | 340 | break; |
|
| - | 341 | case 1: |
|
| - | 342 | I2C_WriteByte(Motor2); |
|
| - | 343 | break; |
|
| - | 344 | case 2: |
|
| - | 345 | I2C_WriteByte(Motor3); |
|
| - | 346 | break; |
|
| - | 347 | case 3: |
|
| - | 348 | I2C_WriteByte(Motor4); |
|
| - | 349 | break; |
|
| - | 350 | case 5: |
|
| - | 351 | I2C_WriteByte(Motor5); |
|
| - | 352 | break; |
|
| - | 353 | case 6: |
|
| - | 354 | I2C_WriteByte(Motor6); |
|
| - | 355 | break; |
|
| - | 356 | case 7: |
|
| - | 357 | I2C_WriteByte(Motor7); |
|
| - | 358 | break; |
|
| - | 359 | case 8: |
|
| - | 360 | I2C_WriteByte(Motor8); |
|
| - | 361 | break; |
|
| - | 362 | } |
|
| - | 363 | break; |
|
| - | 364 | case 2: // repeat case 0+1 for all motors |
|
| - | 365 | I2C_Stop(); |
|
| - | 366 | if (motor_write < 7) |
|
| - | 367 | { |
|
| - | 368 | motor_write++; // jump to next motor |
|
| - | 369 | twi_state = 0; // and repeat from state 0 |
|
| - | 370 | } |
|
| - | 371 | else |
|
| - | 372 | { // data to last motor send |
|
| - | 373 | motor_write = 0; // reset motor write counter |
|
| - | 374 | } |
|
| - | 375 | I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive |
|
| - | 376 | break; |
|
| - | 377 | ||
| - | 378 | // Master Receive |
|
| - | 379 | case 3: // Send SLA-R |
|
| - | 380 | I2C_WriteByte(0x53 + (motor_read * 2) ); |
|
| - | 381 | break; |
|
| - | 382 | case 4: |
|
| - | 383 | //Transmit 1st byte |
|
| - | 384 | I2C_ReceiveByte(); |
|
| - | 385 | break; |
|
| - | 386 | case 5: //Read 1st byte and transmit 2nd Byte |
|
| - | 387 | motor_rx[motor_read] = TWDR; |
|
| - | 388 | I2C_ReceiveLastByte(); |
|
| - | 389 | break; |
|
| - | 390 | case 6: |
|
| - | 391 | //Read 2nd byte |
|
| - | 392 | motor_rx[motor_read + 8] = TWDR; |
|
| - | 393 | motor_read++; |
|
| - | 394 | if (motor_read > 7) motor_read = 0; |
|
| - | 395 | I2C_Stop(); |
|
| - | 396 | twi_state = 0; |
|
| - | 397 | I2CTimeout = 10; |
|
| - | 398 | break; |
|
| - | 399 | ||
| - | 400 | // Gyro-Offsets |
|
| - | 401 | case 7: |
|
| - | 402 | I2C_WriteByte(0x98); // Address the DAC |
|
| - | 403 | break; |
|
| - | 404 | ||
| - | 405 | case 8: |
|
| - | 406 | I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
|
| - | 407 | break; |
|
| - | 408 | ||
| - | 409 | case 9: |
|
| - | 410 | switch(dac_channel) |
|
| - | 411 | { |
|
| - | 412 | case 0: |
|
| - | 413 | I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
|
| - | 414 | break; |
|
| - | 415 | case 1: |
|
| - | 416 | I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
|
| - | 417 | break; |
|
| - | 418 | case 2: |
|
| - | 419 | I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
|
| - | 420 | break; |
|
| - | 421 | } |
|
| - | 422 | break; |
|
| - | 423 | ||
| - | 424 | case 10: |
|
| - | 425 | I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80 |
|
| - | 426 | break; |
|
| - | 427 | ||
| - | 428 | case 11: |
|
| - | 429 | I2C_Stop(); |
|
| - | 430 | I2CTimeout = 10; |
|
| - | 431 | // repeat case 7...10 until all DAC Channels are updated |
|
| - | 432 | if(dac_channel < 2) |
|
| - | 433 | { |
|
| - | 434 | dac_channel ++; // jump to next channel |
|
| - | 435 | twi_state = 7; // and repeat from state 7 |
|
| - | 436 | I2C_Start(); // start transmission for next channel |
|
| - | 437 | } |
|
| - | 438 | else |
|
| - | 439 | { // data to last motor send |
|
| - | 440 | dac_channel = 0; // reset dac channel counter |
|
| - | 441 | twi_state = 0; // reset twi_state |
|
| - | 442 | } |
|
| - | 443 | break; |
|
| - | 444 | ||
| - | 445 | default: |
|
| - | 446 | I2C_Stop(); |
|
| - | 447 | twi_state = 0; |
|
| - | 448 | I2CTimeout = 10; |
|
| - | 449 | motor_write = 0; |
|
| - | 450 | motor_read = 0; |
|
| - | 451 | } |
|
| - | 452 | } |
|
| - | 453 | #endif // USE_OCTO, USE_OCTO2 |
|
| - | 454 | ||
| 269 | 455 | ||