Subversion Repositories FlightCtrl

Rev

Rev 1960 | Rev 1967 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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