Subversion Repositories FlightCtrl

Rev

Rev 1322 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1322 Rev 1479
Line 4... Line 4...
4 
#include "main.h"
 4 
#include "main.h"
Line 5... Line 5...
5 
 
 5 
 
6 
volatile unsigned char twi_state = 0;
 6 
volatile unsigned char twi_state = 0;
7 
unsigned char motor = 0;
 7 
unsigned char motor = 0;
8 
unsigned char motorread = 0,MissingMotor = 0;
 - 
 
9 
unsigned char motor_rx[16],motor_rx2[16];
 - 
 
- 
 
 8 
unsigned char motorread = 0,MissingMotor = 0;
10 
unsigned char MotorPresent[MAX_MOTORS];
 9 
 
- 
 
 10 
MotorData_t Motor[MAX_MOTORS];
11 
unsigned char MotorError[MAX_MOTORS];
 11 
 
Line 12... Line 12...
12 
unsigned int I2CError = 0;
 12 
unsigned int I2CError = 0;
13 
 
 13 
 
14 
//############################################################################
 14 
//############################################################################
15 
//Initzialisieren der I2C (TWI) Schnittstelle
 15 
//Initzialisieren der I2C (TWI) Schnittstelle
16 
void i2c_init(void)
 16 
void i2c_init(void)
17 
//############################################################################
 17 
//############################################################################
18 
{
 18 
{
19 
  TWSR = 0;
 19 
  TWSR = 0;
Line 20... Line 20...
20 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 20 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
21 
}
 21 
}
22 
 
 22 
 
23 
//############################################################################
 23 
//############################################################################
24 
//Start I2C
 24 
//Start I2C
25 
void i2c_start(void)
 25 
void i2c_start(void)
26 
//############################################################################
 26 
//############################################################################
Line 36... Line 36...
36 
}
 36 
}
Line 37... Line 37...
37 
 
 37 
 
38 
void i2c_reset(void)
 38 
void i2c_reset(void)
39 
//############################################################################
 39 
//############################################################################
40 
{
 40 
{
41 
                 i2c_stop();                
 41 
                 i2c_stop();
42 
                 twi_state = 0;
 42 
                 twi_state = 0;
43 
                 motor = TWDR;
 43 
                 motor = TWDR;
44 
                 motor = 0;
 44 
                 motor = 0;
45 
                 TWCR = 0x80;
 45 
                 TWCR = 0x80;
Line 54... Line 54...
54 
}
 54 
}
Line 55... Line 55...
55 
 
 55 
 
56 
//############################################################################
 56 
//############################################################################
57 
void i2c_write_byte(char byte)
 57 
void i2c_write_byte(char byte)
58 
//############################################################################
 58 
//############################################################################
59 
{
 59 
{
60 
    TWSR = 0x00;
 60 
    TWSR = 0x00;
61 
    TWDR = byte;
 61 
    TWDR = byte;
62 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 62 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
Line 103... Line 103...
103 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 103 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
104 
// Writing the Data
 104 
// Writing the Data
105 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 105 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
106 
        case 0:
 106 
        case 0:
107 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
 107 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
108 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
108 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
109 
                                {
 109 
                                {
110 
                                 motor = 0;
 110 
                                 motor = 0;
111 
                                 twi_state = 3;
 111 
                                 twi_state = 3;
112 
                                 i2c_write_byte(0x53+(motorread*2));
 112 
                                 i2c_write_byte(0x53+(motorread*2));
113 
                                 }
 113 
                                 }
114 
                                else i2c_write_byte(0x52+(motor*2));
 114 
                                else i2c_write_byte(0x52+(motor*2));
115 
                break;
 115 
                break;
116 
        case 1:
 116 
        case 1:
117 
                i2c_write_byte(Motor[motor++]);
 117 
                i2c_write_byte(Motor[motor++].SetPoint);
118 
                break;
 118 
                break;
119 
        case 2:
 119 
        case 2:
120 
                if(TWSR == 0x30)
 120 
                if(TWSR == 0x30)
121 
                                 {
 121 
                                 {
122 
                                  if(!missing_motor) missing_motor = motor;
 122 
                                  if(!missing_motor) missing_motor = motor;
123 
                                  if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255;
 123 
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
124 
                                 }
 124 
                                 }
125 
                i2c_stop();
 125 
                i2c_stop();
126 
                I2CTimeout = 10;
 126 
                I2CTimeout = 10;
127 
                twi_state = 0;
 127 
                twi_state = 0;
128 
                i2c_start();  
 128 
                i2c_start();
129 
                break;
 129 
                break;
130 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 130 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
131 
// Reading Data
 131 
// Reading Data
132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 132 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
133 
        case 3:
 133 
        case 3:
134 
               //Transmit 1st byte for reading
 134 
               //Transmit 1st byte for reading
135 
                if(TWSR != 0x40)  // Error?
 135 
                if(TWSR != 0x40)  // Error?
136 
                 {
 136 
                 {
137 
                  MotorPresent[motorread] = 0;
 137 
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
138 
                  motorread++;
 138 
                  motorread++;
139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
 139 
                  if(motorread >= MAX_MOTORS) motorread = 0;
140 
                  i2c_stop();
 140 
                  i2c_stop();
141 
                  twi_state = 0;
 141 
                  twi_state = 0;
142 
                 }
 142 
                 }
143 
                 else
 143 
                 else
144 
                 {
 144 
                 {
145 
                  MotorPresent[motorread] = ('1' - '-') + motorread;
 145 
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
146 
                  I2C_ReceiveByte();
 146 
                  I2C_ReceiveByte();
147 
                 }
 147 
                 }
148 
                MissingMotor = missing_motor;
 148 
                MissingMotor = missing_motor;
149 
                missing_motor = 0;
 149 
                missing_motor = 0;
150 
                break;
 150 
                break;
151 
        case 4: //Read 1st byte and transmit 2nd Byte
 151 
        case 4: //Read 1st byte and transmit 2nd Byte
152 
                motor_rx[motorread] = TWDR;
 152 
                Motor[motorread].Current = TWDR;
153 
                I2C_ReceiveLastByte(); //nack
 153 
                I2C_ReceiveLastByte(); //nack
154 
                break;
 154 
                break;
155 
        case 5:
 155 
        case 5:
156 
                //Read 2nd byte
 156 
                //Read 2nd byte
157 
                motor_rx2[motorread++] = TWDR;
 157 
                Motor[motorread].MaxPWM = TWDR;
- 
 
 158 
                                motorread++; // next motor
158 
                if(motorread >= MAX_MOTORS) motorread = 0;
 159 
                if(motorread >= MAX_MOTORS) motorread = 0;
159 
                i2c_stop();
 160 
                i2c_stop();
160 
                twi_state = 0;
 161 
                twi_state = 0;
161 
                break;
 162 
                break;
162 
                       
 163 
 
163 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 164 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
164 
// writing Gyro-Offset
 165 
// writing Gyro-Offset
165 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 166 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166 
        case 8:
 167 
        case 8:
167 
                i2c_write_byte(0x98); // Address of the DAC
 168 
                i2c_write_byte(0x98); // Address of the DAC
168 
                break;
 169 
                break;
169 
        case 9:
 170 
        case 9:
170 
                i2c_write_byte(0x10); // Update Channel A
 171 
                i2c_write_byte(0x10); // Update Channel A
171 
                break;
 172 
                break;
172 
        case 10:
 173 
        case 10:
173 
                i2c_write_byte(AnalogOffsetNick); // Value
 174 
                i2c_write_byte(AnalogOffsetNick); // Value
174 
                break;
 175 
                break;
175 
        case 11:
 176 
        case 11:
176 
                i2c_write_byte(0x80); // Value
 177 
                i2c_write_byte(0x80); // Value
177 
                break;
 178 
                break;
178 
        case 12:
 179 
        case 12:
179 
                i2c_stop();              
 180 
                i2c_stop();
180 
                I2CTimeout = 10;
 181 
                I2CTimeout = 10;
181 
                i2c_start();  
 182 
                i2c_start();
182 
                break;
 183 
                break;
183 
        case 13:
 184 
        case 13:
184 
                i2c_write_byte(0x98); // Address of the DAC
 185 
                i2c_write_byte(0x98); // Address of the DAC
185 
                break;
 186 
                break;
186 
        case 14:
 187 
        case 14:
187 
                i2c_write_byte(0x12); // Update Channel B
 188 
                i2c_write_byte(0x12); // Update Channel B
188 
                break;
 189 
                break;
189 
        case 15:
 190 
        case 15:
190 
                i2c_write_byte(AnalogOffsetRoll); // Value
 191 
                i2c_write_byte(AnalogOffsetRoll); // Value
191 
                break;
 192 
                break;
192 
        case 16:
 193 
        case 16:
193 
                i2c_write_byte(0x80); // Value
 194 
                i2c_write_byte(0x80); // Value
194 
                break;
 195 
                break;
195 
        case 17:
 196 
        case 17:
196 
                i2c_stop();              
 197 
                i2c_stop();
197 
                I2CTimeout = 10;
 198 
                I2CTimeout = 10;
198 
                i2c_start();  
 199 
                i2c_start();
199 
                break;
 200 
                break;
200 
        case 18:
 201 
        case 18:
201 
                i2c_write_byte(0x98); // Address of the DAC
 202 
                i2c_write_byte(0x98); // Address of the DAC
202 
                break;
 203 
                break;
203 
        case 19:
 204 
        case 19:
204 
                i2c_write_byte(0x14); // Update Channel C
 205 
                i2c_write_byte(0x14); // Update Channel C
205 
                break;
 206 
                break;
206 
        case 20:
 207 
        case 20:
207 
                i2c_write_byte(AnalogOffsetGier); // Value
 208 
                i2c_write_byte(AnalogOffsetGier); // Value
208 
                break;
 209 
                break;
209 
        case 21:
 210 
        case 21:
210 
                i2c_write_byte(0x80); // Value
 211 
                i2c_write_byte(0x80); // Value
211 
                break;
 212 
                break;
212 
        case 22:
 213 
        case 22:
213 
                i2c_stop();              
 214 
                i2c_stop();
214 
                I2CTimeout = 10;
 215 
                I2CTimeout = 10;
215 
                twi_state = 0;
 216 
                twi_state = 0;
216 
                break;
 217 
                break;
217 
        default: twi_state = 0;
 218 
        default: twi_state = 0;
218 
                break;        
 219 
                break;
219 
                }
 220 
                }
220 
 TWCR |= 0x80;
 221 
 TWCR |= 0x80;
221 
}
 222 
}