Subversion Repositories FlightCtrl

Rev

Rev 1479 | Rev 1639 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1479 Rev 1638
1 
/*############################################################################
 1 
/*############################################################################
2 
############################################################################*/
 2 
############################################################################*/
3 
 
 3 
 
4 
#include "main.h"
 4 
#include "main.h"
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;
 8 
unsigned char motorread = 0,MissingMotor = 0;
9 
 
 9 
 
10 
MotorData_t Motor[MAX_MOTORS];
 10 
MotorData_t Motor[MAX_MOTORS];
11 
 
 11 
 
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;
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 
//############################################################################
27 
{
 27 
{
28 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
 28 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
29 
}
 29 
}
30 
 
 30 
 
31 
//############################################################################
 31 
//############################################################################
32 
void i2c_stop(void)
 32 
void i2c_stop(void)
33 
//############################################################################
 33 
//############################################################################
34 
{
 34 
{
35 
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
 35 
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
36 
}
 36 
}
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;
46 
                 TWAMR = 0;
 46 
                 TWAMR = 0;
47 
                 TWAR = 0;
 47 
                 TWAR = 0;
48 
                 TWDR = 0;
 48 
                 TWDR = 0;
49 
                 TWSR = 0;
 49 
                 TWSR = 0;
50 
                 TWBR = 0;
 50 
                 TWBR = 0;
51 
                 i2c_init();
 51 
                 i2c_init();
52 
                 i2c_start();
 52 
                 i2c_start();
53 
                 i2c_write_byte(0);
 53 
                 i2c_write_byte(0);
54 
}
 54 
}
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);
63 
}
 63 
}
64 
 
 64 
 
65 
/****************************************/
 65 
/****************************************/
66 
/*    Write to I2C                      */
 66 
/*    Write to I2C                      */
67 
/****************************************/
 67 
/****************************************/
68 
void I2C_WriteByte(int8_t byte)
 68 
void I2C_WriteByte(int8_t byte)
69 
{
 69 
{
70 
    // move byte to send into TWI Data Register
 70 
    // move byte to send into TWI Data Register
71 
    TWDR = byte;
 71 
    TWDR = byte;
72 
    // clear interrupt flag (TWINT = 1)
 72 
    // clear interrupt flag (TWINT = 1)
73 
    // enable i2c bus (TWEN = 1)
 73 
    // enable i2c bus (TWEN = 1)
74 
    // enable interrupt (TWIE = 1)
 74 
    // enable interrupt (TWIE = 1)
75 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 75 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
76 
}
 76 
}
77 
 
 77 
 
78 
/****************************************/
 78 
/****************************************/
79 
/*    Receive byte and send ACK         */
 79 
/*    Receive byte and send ACK         */
80 
/****************************************/
 80 
/****************************************/
81 
void I2C_ReceiveByte(void)
 81 
void I2C_ReceiveByte(void)
82 
{
 82 
{
83 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
 83 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
84 
}
 84 
}
85 
 
 85 
 
86 
/****************************************/
 86 
/****************************************/
87 
/* I2C receive last byte and send no ACK*/
 87 
/* I2C receive last byte and send no ACK*/
88 
/****************************************/
 88 
/****************************************/
89 
void I2C_ReceiveLastByte(void)
 89 
void I2C_ReceiveLastByte(void)
90 
{
 90 
{
91 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 91 
   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
92 
}
 92 
}
93 
 
 93 
 
94 
 
 94 
 
95 
 
 95 
/*
96 
//############################################################################
 96 
//############################################################################
97 
SIGNAL (TWI_vect)
 97 
SIGNAL (TWI_vect)
98 
//############################################################################
 98 
//############################################################################
99 
{
 99 
{
100 
 static unsigned char missing_motor;
 100 
 static unsigned char missing_motor;
101 
     switch(twi_state++)
 101 
     switch(twi_state++)
102 
        {
 102 
        {
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++].SetPoint);
 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((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
 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 
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
 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 
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
 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[motorread].Current = 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[motorread].MaxPWM = TWDR;
 157 
                Motor[motorread].MaxPWM = TWDR;
158 
                                motorread++; // next motor
 158 
                                motorread++; // next motor
159 
                if(motorread >= MAX_MOTORS) motorread = 0;
 159 
                if(motorread >= MAX_MOTORS) motorread = 0;
160 
                i2c_stop();
 160 
                i2c_stop();
161 
                twi_state = 0;
 161 
                twi_state = 0;
162 
                break;
 162 
                break;
163 
 
 163 
 
164 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 164 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
165 
// writing Gyro-Offset
 165 
// writing Gyro-Offset
166 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 166 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
167 
        case 8:
 167 
        case 8:
168 
                i2c_write_byte(0x98); // Address of the DAC
 168 
                i2c_write_byte(0x98); // Address of the DAC
169 
                break;
 169 
                break;
170 
        case 9:
 170 
        case 9:
171 
                i2c_write_byte(0x10); // Update Channel A
 171 
                i2c_write_byte(0x10); // Update Channel A
172 
                break;
 172 
                break;
173 
        case 10:
 173 
        case 10:
174 
                i2c_write_byte(AnalogOffsetNick); // Value
 174 
                i2c_write_byte(AnalogOffsetNick); // Value
175 
                break;
 175 
                break;
176 
        case 11:
 176 
        case 11:
177 
                i2c_write_byte(0x80); // Value
 177 
                i2c_write_byte(0x80); // Value
178 
                break;
 178 
                break;
179 
        case 12:
 179 
        case 12:
180 
                i2c_stop();
 180 
                i2c_stop();
181 
                I2CTimeout = 10;
 181 
                I2CTimeout = 10;
182 
                i2c_start();
 182 
                i2c_start();
183 
                break;
 183 
                break;
184 
        case 13:
 184 
        case 13:
185 
                i2c_write_byte(0x98); // Address of the DAC
 185 
                i2c_write_byte(0x98); // Address of the DAC
186 
                break;
 186 
                break;
187 
        case 14:
 187 
        case 14:
188 
                i2c_write_byte(0x12); // Update Channel B
 188 
                i2c_write_byte(0x12); // Update Channel B
189 
                break;
 189 
                break;
190 
        case 15:
 190 
        case 15:
191 
                i2c_write_byte(AnalogOffsetRoll); // Value
 191 
                i2c_write_byte(AnalogOffsetRoll); // Value
192 
                break;
 192 
                break;
193 
        case 16:
 193 
        case 16:
194 
                i2c_write_byte(0x80); // Value
 194 
                i2c_write_byte(0x80); // Value
195 
                break;
 195 
                break;
196 
        case 17:
 196 
        case 17:
197 
                i2c_stop();
 197 
                i2c_stop();
198 
                I2CTimeout = 10;
 198 
                I2CTimeout = 10;
199 
                i2c_start();
 199 
                i2c_start();
200 
                break;
 200 
                break;
201 
        case 18:
 201 
        case 18:
202 
                i2c_write_byte(0x98); // Address of the DAC
 202 
                i2c_write_byte(0x98); // Address of the DAC
203 
                break;
 203 
                break;
204 
        case 19:
 204 
        case 19:
205 
                i2c_write_byte(0x14); // Update Channel C
 205 
                i2c_write_byte(0x14); // Update Channel C
206 
                break;
 206 
                break;
207 
        case 20:
 207 
        case 20:
208 
                i2c_write_byte(AnalogOffsetGier); // Value
 208 
                i2c_write_byte(AnalogOffsetGier); // Value
209 
                break;
 209 
                break;
210 
        case 21:
 210 
        case 21:
211 
                i2c_write_byte(0x80); // Value
 211 
                i2c_write_byte(0x80); // Value
212 
                break;
 212 
                break;
213 
        case 22:
 213 
        case 22:
214 
                i2c_stop();
 214 
                i2c_stop();
- 
 
 215 
                I2CTimeout = 10;
- 
 
 216 
                twi_state = 0;
- 
 
 217 
                break;
- 
 
 218 
        default: twi_state = 0;
- 
 
 219 
                break;
- 
 
 220 
                }
- 
 
 221 
 TWCR |= 0x80;
- 
 
 222 
}
- 
 
 223 
*/
- 
 
 224 
 
- 
 
 225 
/*
- 
 
 226 
                if(DataArray[3] & 0x01) Parameter.PwmScaling            = DataArray[4];
- 
 
 227 
                if(DataArray[3] & 0x02) Parameter.MaxStrom                      = DataArray[5];
- 
 
 228 
                if(DataArray[3] & 0x04) Parameter.TemperaturLimiter = DataArray[6];
- 
 
 229 
                if(DataArray[3] & 0x08) Parameter.SkaliereStrom         = DataArray[7];
- 
 
 230 
                if(DataArray[3] & 0x10) Parameter.Bitconfig                     = DataArray[8];
- 
 
 231 
*/
- 
 
 232 
 
- 
 
 233 
//############################################################################
- 
 
 234 
SIGNAL (TWI_vect)
- 
 
 235 
//############################################################################
- 
 
 236 
{                           // 2    3   4   5   6   7   8  9
- 
 
 237 
 unsigned char test[] = {0,0,'#',0x1F,255,30,100,64,0x00,7,8,9,10};
- 
 
 238 
 static unsigned char missing_motor,send = 0,crc = 0;
- 
 
 239 
     switch(twi_state++)
- 
 
 240 
        {
- 
 
 241 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 242 
// Writing the Data
- 
 
 243 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 244 
        case 0:
- 
 
 245 
                        while(Mixer.Motor[motor][0] <= 0 && motor < MAX_MOTORS) motor++;  // skip if not used
- 
 
 246 
                                if(motor == MAX_MOTORS)  // writing finished -> now read
- 
 
 247 
                                {
- 
 
 248 
                                 motor = 0;
- 
 
 249 
                                 twi_state = 4;
- 
 
 250 
                                 i2c_write_byte(0x53+(motorread*2));
- 
 
 251 
                                 }
- 
 
 252 
                                else i2c_write_byte(0x52+(motor*2));
- 
 
 253 
                                send = 0;
- 
 
 254 
                break;
- 
 
 255 
        case 1:
- 
 
 256 
                i2c_write_byte(Motor[motor].SetPoint);
- 
 
 257 
                if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) twi_state++;
- 
 
 258 
                break;
- 
 
 259 
        case 2:
- 
 
 260 
                if(!send++)
- 
 
 261 
                                 {
- 
 
 262 
                                  i2c_write_byte((Motor[motor].SetPointLowerBits << 1));// + (7 << 0));
- 
 
 263 
                                  crc = 0xAA;
- 
 
 264 
                                 }
- 
 
 265 
                                else
- 
 
 266 
                if(send == 9) i2c_write_byte(crc);
- 
 
 267 
                                else          
- 
 
 268 
                                 {
- 
 
 269 
                                  crc += test[send];
- 
 
 270 
                                  i2c_write_byte(test[send]);
- 
 
 271 
                                }
- 
 
 272 
                if(!MotorenEin && motor == motorread  && !PC_MotortestActive && (Motor[motorread].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) if(send <= 10) twi_state--;
- 
 
 273 
                break;
- 
 
 274 
        case 3:
- 
 
 275 
                        motor++;
- 
 
 276 
                if(TWSR == 0x30)
- 
 
 277 
                                 {
- 
 
 278 
                                  if(!missing_motor) missing_motor = motor;
- 
 
 279 
                                  if((Motor[motor-1].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor-1].State++; // increment error counter and handle overflow
- 
 
 280 
                                 }
- 
 
 281 
                i2c_stop();
- 
 
 282 
                I2CTimeout = 10;
- 
 
 283 
                twi_state = 0;
- 
 
 284 
                i2c_start();
- 
 
 285 
                break;
- 
 
 286 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 287 
// Reading Data
- 
 
 288 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 289 
        case 4:
- 
 
 290 
               //Transmit 1st byte for reading
- 
 
 291 
                if(TWSR != 0x40)  // Error?
- 
 
 292 
                 {
- 
 
 293 
                  Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
- 
 
 294 
                  motorread++;
- 
 
 295 
                  if(motorread >= MAX_MOTORS) motorread = 0;
- 
 
 296 
                  i2c_stop();
- 
 
 297 
                  twi_state = 0;
- 
 
 298 
                 }
- 
 
 299 
                 else
- 
 
 300 
                 {
- 
 
 301 
                  Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
- 
 
 302 
                  I2C_ReceiveByte();
- 
 
 303 
                 }
- 
 
 304 
                MissingMotor = missing_motor;
- 
 
 305 
                missing_motor = 0;
- 
 
 306 
                break;
- 
 
 307 
        case 5: //Read 1st byte and transmit 2nd Byte
- 
 
 308 
                Motor[motorread].Current = TWDR;
- 
 
 309 
                I2C_ReceiveByte(); //nack
- 
 
 310 
                break;
- 
 
 311 
        case 6:
- 
 
 312 
                       //Read 2nd byte and transmit 3rd Byte
- 
 
 313 
                Motor[motorread].MaxPWM = TWDR;
- 
 
 314 
                                if(TWDR == 250)  
- 
 
 315 
                                 {
- 
 
 316 
                                  //if(!Motor[motor].SetPoint)
- 
 
 317 
                                  Motor[motorread].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
- 
 
 318 
                                 }
- 
 
 319 
                                I2C_ReceiveLastByte(); //nack
- 
 
 320 
                break;
- 
 
 321 
        case 7: // read next
- 
 
 322 
                                if(TWDR == 255) { if(!MotorenEin) Motor[motorread].Version = 0; DebugOut.Analog[24]++;}
- 
 
 323 
                                Motor[motorread].Temperature = TWDR;
- 
 
 324 
                                motorread++; // next motor
- 
 
 325 
                if(motorread >= MAX_MOTORS) { motorread = 0; }
- 
 
 326 
                i2c_stop();
- 
 
 327 
                twi_state = 0;
- 
 
 328 
                break;
- 
 
 329 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 330 
// writing Gyro-Offset
- 
 
 331 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 332 
        case 18:
- 
 
 333 
                i2c_write_byte(0x98); // Address of the DAC
- 
 
 334 
                break;
- 
 
 335 
        case 19:
- 
 
 336 
                i2c_write_byte(0x10); // Update Channel A
- 
 
 337 
                break;
- 
 
 338 
        case 20:
- 
 
 339 
                i2c_write_byte(AnalogOffsetNick); // Value
- 
 
 340 
                break;
- 
 
 341 
        case 21:
- 
 
 342 
                i2c_write_byte(0x80); // Value
- 
 
 343 
                break;
- 
 
 344 
        case 22:
- 
 
 345 
                i2c_stop();
- 
 
 346 
                I2CTimeout = 10;
- 
 
 347 
                i2c_start();
- 
 
 348 
                break;
- 
 
 349 
        case 23:
- 
 
 350 
                i2c_write_byte(0x98); // Address of the DAC
- 
 
 351 
                break;
- 
 
 352 
        case 24:
- 
 
 353 
                i2c_write_byte(0x12); // Update Channel B
- 
 
 354 
                break;
- 
 
 355 
        case 25:
- 
 
 356 
                i2c_write_byte(AnalogOffsetRoll); // Value
- 
 
 357 
                break;
- 
 
 358 
        case 26:
- 
 
 359 
                i2c_write_byte(0x80); // Value
- 
 
 360 
                break;
- 
 
 361 
        case 27:
- 
 
 362 
                i2c_stop();
- 
 
 363 
                I2CTimeout = 10;
- 
 
 364 
                i2c_start();
- 
 
 365 
                break;
- 
 
 366 
        case 28:
- 
 
 367 
                i2c_write_byte(0x98); // Address of the DAC
- 
 
 368 
                break;
- 
 
 369 
        case 29:
- 
 
 370 
                i2c_write_byte(0x14); // Update Channel C
- 
 
 371 
                break;
- 
 
 372 
        case 30:
- 
 
 373 
                i2c_write_byte(AnalogOffsetGier); // Value
- 
 
 374 
                break;
- 
 
 375 
        case 31:
- 
 
 376 
                i2c_write_byte(0x80); // Value
- 
 
 377 
                break;
- 
 
 378 
        case 32:
- 
 
 379 
                i2c_stop();
215 
                I2CTimeout = 10;
 380 
                I2CTimeout = 10;
216 
                twi_state = 0;
 381 
                twi_state = 0;
217 
                break;
 382 
                break;
218 
        default: twi_state = 0;
 383 
        default: twi_state = 0;
219 
                break;
 384 
                break;
220 
                }
 385 
                }
221 
 TWCR |= 0x80;
 386 
 TWCR |= 0x80;
222 
}
 387 
}
223 
 
 388