Subversion Repositories FlightCtrl

Rev

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