Subversion Repositories FlightCtrl

Rev

Rev 839 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 839 Rev 966
Line 16... Line 16...
16 
//############################################################################
 16 
//############################################################################
17 
//Initzialisieren der I2C (TWI) Schnittstelle
 17 
//Initzialisieren der I2C (TWI) Schnittstelle
18 
void i2c_init(void)
 18 
void i2c_init(void)
19 
//############################################################################
 19 
//############################################################################
20 
{
 20 
{
21 
        TWSR = 0;
 21 
  TWSR = 0;
22 
        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 22 
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
23 
}
 23 
}
Line 24... Line 24...
24 
 
 24 
 
25 
//############################################################################
 25 
//############################################################################
26 
//Start I2C
 26 
//Start I2C
27 
char i2c_start(void)
 27 
char i2c_start(void)
28 
//############################################################################
 28 
//############################################################################
29 
{
 29 
{
30 
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
 30 
  TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
31 
    return(0);
 31 
  return(0);
Line 32... Line 32...
32 
}
 32 
}
33 
 
 33 
 
34 
//############################################################################
 34 
//############################################################################
35 
//Start I2C
 35 
//Start I2C
36 
void i2c_stop(void)
 36 
void i2c_stop(void)
37 
//############################################################################
 37 
//############################################################################
38 
{
 38 
{
Line 39... Line 39...
39 
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
 39 
  TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
40 
}
 40 
}
41 
 
 41 
 
42 
void i2c_reset(void)
 42 
void i2c_reset(void)
43 
//############################################################################
 43 
//############################################################################
44 
{
 44 
{
45 
        i2c_stop();                
 45 
  i2c_stop();                
46 
        twi_state = 0;
 46 
  twi_state = 0;
47 
        motor = TWDR;
 47 
  motor = TWDR;
48 
        motor = 0;
 48 
  motor = 0;
49 
        TWCR = 0x80;
 49 
  TWCR = 0x80;
50 
        TWAMR = 0;
 50 
  TWAMR = 0;
51 
        TWAR = 0;
 51 
  TWAR = 0;
52 
        TWDR = 0;
 52 
  TWDR = 0;
53 
        TWSR = 0;
 53 
  TWSR = 0;
54 
        TWBR = 0;
 54 
  TWBR = 0;
55 
        i2c_init();
 55 
  i2c_init();
Line 56... Line 56...
56 
        i2c_start();
 56 
  i2c_start();
57 
        i2c_write_byte(0);
 57 
  i2c_write_byte(0);
58 
}
 58 
}
59 
 
 59 
 
60 
//############################################################################
 60 
//############################################################################
61 
//Start I2C
 61 
//Start I2C
62 
char i2c_write_byte(char byte)
 62 
char i2c_write_byte(char byte)
63 
//############################################################################
 63 
//############################################################################
64 
{
 64 
{
65 
    TWSR = 0x00;
 65 
  TWSR = 0x00;
66 
    TWDR = byte;
 66 
  TWDR = byte;
Line 67... Line 67...
67 
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
 67 
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
68 
   
 68 
 
69 
    return(0);    
 69 
  return(0);    
70 
}
 70 
}
71 
 
 71 
 
72 
//############################################################################
 72 
//############################################################################
73 
//Start I2C
 73 
//Start I2C
74 
SIGNAL (TWI_vect)
 74 
SIGNAL (TWI_vect)
75 
//############################################################################
 75 
//############################################################################
76 
{
 76 
{
190 
                        }
 189 
      }
191 
#endif                                                                 
190 
#endif        
192 
                        i2c_write_byte(Motor_Rechts);
 191 
      i2c_write_byte(Motor_Rechts);
193 
                        break;
 192 
      break;
194 
                case 3:        
 193 
    case 3:  
195 
#if 0
 194 
#if 0
196 
                        pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
 195 
      pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
197 
                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
 196 
      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
198 
                        {
 197 
      {
199 
                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
 198 
        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
200 
                        }
 199 
      }
201 
                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
 200 
      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
202 
                        {
 201 
      {
203 
                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
 202 
        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
204 
                        }
 203 
      }
205 
                        /* M o t o r   L i n k s */
 204 
      /* M o t o r   L i n k s */
206 
                        motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 205 
      motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
207 
                        if (motorwert < 0)
 206 
      if (motorwert < 0)
208 
                        {
 207 
      {
209 
                                motorwert = 0;
 208 
        motorwert = 0;
210 
                        }
 209 
      }
211 
                        else if(motorwert > MAX_GAS)
 210 
      else if(motorwert > MAX_GAS)
212 
                        {
 211 
      {
213 
                                motorwert = MAX_GAS;
 212 
        motorwert = MAX_GAS;
214 
                        }
 213 
      }
215 
                        if (motorwert < MIN_GAS)
 214 
      if (motorwert < MIN_GAS)
216 
                        {
 215 
      {
217 
                                motorwert = MIN_GAS;
 216 
        motorwert = MIN_GAS;
218 
                        }
 217 
      }
219 
                        Motor_Links = motorwert;                       
 218 
      Motor_Links = motorwert;  
220 
                        if(!MotorenEin)
 219 
      if(!MotorenEin)
221 
                        {
 220 
      {
- 
 
 221 
        Motor_Links = 0;
- 
 
 222 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
222 
                                Motor_Links = 0;
 223 
      }
- 
 
 224 
#endif    
- 
 
 225 
      i2c_write_byte(Motor_Links);
- 
 
 226 
      break;
223 
                                if(MotorTest[2]) Motor_Links = MotorTest[2];
 227 
    }
- 
 
 228 
    break;
- 
 
 229 
    case 2:
- 
 
 230 
      i2c_stop();
- 
 
 231 
      if (motor<4) twi_state = 0;
- 
 
 232 
      else motor = 0;
- 
 
 233 
      i2c_start();  
- 
 
 234 
      break;
- 
 
 235 
     
- 
 
 236 
      //Liest Daten von Motor
- 
 
 237 
    case 3:
- 
 
 238 
      i2c_write_byte(0x53+(motorread*2));
- 
 
 239 
      break;
- 
 
 240 
    case 4:
- 
 
 241 
      switch(motorread)
- 
 
 242 
      {
- 
 
 243 
      case 0:
- 
 
 244 
        i2c_write_byte(Motor_Vorne);
- 
 
 245 
        break;
- 
 
 246 
      case 1:
- 
 
 247 
        i2c_write_byte(Motor_Hinten);
- 
 
 248 
        break;
- 
 
 249 
      case 2:
- 
 
 250 
        i2c_write_byte(Motor_Rechts);
224 
                        }
 251 
        break;
- 
 
 252 
      case 3:
- 
 
 253 
        i2c_write_byte(Motor_Links);
- 
 
 254 
        break;
- 
 
 255 
      }
- 
 
 256 
      break;
- 
 
 257 
      case 5: //1 Byte vom Motor lesen      
- 
 
 258 
        motor_rx[motorread] = TWDR;
- 
 
 259 
       
- 
 
 260 
      case 6:
- 
 
 261 
        switch(motorread)
- 
 
 262 
        {
- 
 
 263 
        case 0:
225 
#endif                         
264 
          i2c_write_byte(Motor_Vorne);
226 
                        i2c_write_byte(Motor_Links);
 - 
 
227 
                        break;
 265 
          break;
228 
                    }
 - 
 
229 
                                        break;
 - 
 
230 
        case 2:
 266 
        case 1:
231 
                        i2c_stop();
 - 
 
232 
                        if (motor<4) twi_state = 0;
 - 
 
233 
                        else motor = 0;
 267 
          i2c_write_byte(Motor_Hinten);
419 
                                        UltrasonicRangeLow = TWDR;             
 418 
          UltrasonicRangeLow = TWDR;  
420 
                                        i2c_stop();
 419 
          i2c_stop();
421 
                                        I2CTimeout = 22;
 420 
          I2CTimeout = 22;
422 
                                        twi_state = 0;
 421 
          twi_state = 0;