WebSVN - FlightCtrl - Diff - Rev 839 and 966 - /branches/KalmanFilter MikeW/twimaster.c

 /*############################################################################
 ############################################################################*/
 #include "main.h"
 #include "kafi.h"
 unsigned char twi_state = 0;
 unsigned char motor = 0;
 unsigned char motorread = 0;
 unsigned char motor_rx[8];
 unsigned int TriggerMagneticHeading = 0;
 unsigned int TriggerSonicReading = 0;
 //############################################################################
 //Initzialisieren der I2C (TWI) Schnittstelle
 void i2c_init(void)
 //############################################################################
+{
-        TWSR = 0;
+  TWSR = 0;
-        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
+  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
+}
 //############################################################################
 //Start I2C
 char i2c_start(void)
 //############################################################################
+{
-    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
+  TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
-    return(0);
+  return(0);
+}
 //############################################################################
 //Start I2C
 void i2c_stop(void)
 //############################################################################
+{
-    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
+  TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
+}
 void i2c_reset(void)
 //############################################################################
+{
-        i2c_stop();
+  i2c_stop();
-        twi_state = 0;
+  twi_state = 0;
-        motor = TWDR;
+  motor = TWDR;
-        motor = 0;
+  motor = 0;
-        TWCR = 0x80;
+  TWCR = 0x80;
-        TWAMR = 0;
+  TWAMR = 0;
-        TWAR = 0;
+  TWAR = 0;
-        TWDR = 0;
+  TWDR = 0;
-        TWSR = 0;
+  TWSR = 0;
-        TWBR = 0;
+  TWBR = 0;
-        i2c_init();
+  i2c_init();
-        i2c_start();
+  i2c_start();
-        i2c_write_byte(0);
+  i2c_write_byte(0);
+}
 //############################################################################
 //Start I2C
 char i2c_write_byte(char byte)
 //############################################################################
+{
-    TWSR = 0x00;
+  TWSR = 0x00;
-    TWDR = byte;
+  TWDR = byte;
-    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
+  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
-    return(0);
+  return(0);
+}
 //############################################################################
 //Start I2C
 SIGNAL (TWI_vect)
 //############################################################################
+{
 #if 0
-        int motorwert = 0;
+  int motorwert = 0;
-        int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F)  / 6;
+  int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F)  / 6;
-    int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F)  / 6;
+  int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F)  / 6;
 #endif
 #if 0
-                        pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
+      pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
-                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
+      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
-                        {
+      {
-                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
+        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
-                        }
+      }
-                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
+      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
-                        {
+      {
-                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
+        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
-                        }
+      }
-                        /* M o t o r   L i n k s */
+      /* M o t o r   L i n k s */
-                        motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
+      motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
-                        if (motorwert < 0)
+      if (motorwert < 0)
-                        {
+      {
-                                motorwert = 0;
+        motorwert = 0;
-                        }
+      }
-                        else if(motorwert > MAX_GAS)
+      else if(motorwert > MAX_GAS)
-                        {
+      {
-                                motorwert = MAX_GAS;
+        motorwert = MAX_GAS;
-                        }
+      }
-                        if (motorwert < MIN_GAS)
+      if (motorwert < MIN_GAS)
-                        {
+      {
-                                motorwert = MIN_GAS;
+        motorwert = MIN_GAS;
-                        }
+      }
-                        Motor_Links = motorwert;
+      Motor_Links = motorwert;
-                        if(!MotorenEin)
+      if(!MotorenEin)
-                        {
+      {
-                                Motor_Links = 0;
+        Motor_Links = 0;
-                                if(MotorTest[2]) Motor_Links = MotorTest[2];
+        if(MotorTest[2]) Motor_Links = MotorTest[2];
-                        }
+      }
 #endif
-                        i2c_write_byte(Motor_Links);
+      i2c_write_byte(Motor_Links);
+      break;
+    }
-                        break;
+    break;
+    case 2:
+      i2c_stop();
+      if (motor<4) twi_state = 0;
-                    }
+      else motor = 0;
+      i2c_start();
+      break;
+      //Liest Daten von Motor
+    case 3:
+      i2c_write_byte(0x53+(motorread*2));
+      break;
+    case 4:
+      switch(motorread)
+      {
+      case 0:
+        i2c_write_byte(Motor_Vorne);
+        break;
+      case 1:
+        i2c_write_byte(Motor_Hinten);
+        break;
+      case 2:
+        i2c_write_byte(Motor_Rechts);
+        break;
+      case 3:
+        i2c_write_byte(Motor_Links);
+        break;
+      }
-                                        break;
+      break;
+      case 5: //1 Byte vom Motor lesen
+        motor_rx[motorread] = TWDR;
+      case 6:
+        switch(motorread)
+        {
+        case 0:
+          i2c_write_byte(Motor_Vorne);
+          break;
+        case 1:
+          i2c_write_byte(Motor_Hinten);
+          break;
         case 2:
-                        i2c_stop();
-                        if (motor<4) twi_state = 0;
+          i2c_write_byte(Motor_Rechts);
-                        else motor = 0;
-                        i2c_start();
-                        break;
+          break;
-                        //Liest Daten von Motor
         case 3:
+    }
 #endif
-        TWCR |= 0x80;
+    TWCR |= 0x80;
+ }

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/twimaster.c @ 2253 - Rev 839 → 966