WebSVN - FlightCtrl - Rev 883 - /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;
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
}

//############################################################################
//Start I2C
char i2c_start(void)
//############################################################################
{
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
return(0);
}

//############################################################################
//Start I2C
void i2c_stop(void)
//############################################################################
{
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
}

void i2c_reset(void)
//############################################################################
{
i2c_stop();
twi_state = 0;
motor = TWDR;
motor = 0;
TWCR = 0x80;
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
i2c_init();
i2c_start();
i2c_write_byte(0);
}

//############################################################################
//Start I2C
char i2c_write_byte(char byte)
//############################################################################
{
TWSR = 0x00;
TWDR = byte;
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);

return(0);
}

//############################################################################
//Start I2C
SIGNAL (TWI_vect)
//############################################################################
{
#if 0
int motorwert = 0;
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;
#endif

switch (twi_state++)
{
case 0:
i2c_write_byte(0x52+(motor*2));
break;
case 1:
switch(motor++)
{
case 0:
#if 0
pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r V o r n */
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Vorne = motorwert;
if(!MotorenEin)
{
Motor_Vorne = 0;
if(MotorTest[0]) Motor_Vorne = MotorTest[0];
}
#endif
i2c_write_byte(Motor_Vorne);
break;
case 1:
#if 0
pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r H e c k */
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
if ((motorwert < 0))
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Hinten = motorwert;

if(!MotorenEin)
{
Motor_Hinten = 0;
if(MotorTest[1]) Motor_Hinten = MotorTest[1];
}
#endif
i2c_write_byte(Motor_Hinten);
break;
case 2:
#if 0
pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r R e c h t s */
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
if (motorwert < 0)
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Rechts = motorwert;
if(!MotorenEin)
{
Motor_Rechts = 0;
if(MotorTest[3]) Motor_Rechts = MotorTest[3];
}
#endif
i2c_write_byte(Motor_Rechts);
break;
case 3:
#if 0
pd_ergebnis = (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = (GasMischanteil + abs(GierMischanteil));
}
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
{
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
}
/* M o t o r L i n k s */
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
if (motorwert < 0)
{
motorwert = 0;
}
else if(motorwert > MAX_GAS)
{
motorwert = MAX_GAS;
}
if (motorwert < MIN_GAS)
{
motorwert = MIN_GAS;
}
Motor_Links = motorwert;
if(!MotorenEin)
{
Motor_Links = 0;
if(MotorTest[2]) Motor_Links = MotorTest[2];
}
#endif
i2c_write_byte(Motor_Links);
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;
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_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 7: //2 Byte vom Motor lesen
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread>3) motorread=0;
i2c_stop();
I2CTimeout = 10;
twi_state = 0;
}
#if 0
break;
TriggerMagneticHeading++;
TriggerMagneticHeading %= 50;
TriggerSonicReading++;
TriggerSonicReading %= 200;

if (TriggerMagneticHeading == 1)
{
// Get Magnetic Heading
i2c_start();
twi_state = 8;
break;
}

if (TriggerSonicReading == 1)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 20;
break;
}
else if (TriggerSonicReading == 190)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 30;
break;
}
else
{
I2CTimeout = 10;
twi_state = 0;
break;
}

case 8:
i2c_write_byte(0xC0);
break;
case 9:
i2c_write_byte(0x02);
break;
case 10:
i2c_start();
break;
case 11:
i2c_write_byte(0xC1);
break;
case 12:
i2c_write_byte(MagneticHeading);
break;
case 13:
MagneticHeading = (0x0F & TWDR) << 8;
i2c_stop();
i2c_start();
break;
case 14:
i2c_write_byte(0xC0);
break;
case 15:
i2c_write_byte(0x03);
break;
case 16:
i2c_start();
break;
case 17:
i2c_write_byte(0xC1);
break;
case 18:
i2c_write_byte(MagneticHeading);
break;
case 19:
MagneticHeading += TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
break;

/* Trigger Ultrasonic Ping */
case 20: /* Ping */
i2c_write_byte(0xE0); /* Send I2C Adress */
break;
case 21:
i2c_write_byte(0x00); /* Send I2C Register */
break;
case 22:
i2c_write_byte(82); /* Send I2C Value */
break;
case 24:
VersionID = TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
UltrasonicPingCnt++;
break;
case 30:
i2c_write_byte(0xE0);/* Send I2C Adress */
break;
case 31:
i2c_write_byte(0x02); /* Send I2C Register */
break;
case 32:
i2c_start();
break;
case 33:
i2c_write_byte(0xE1);
break;
case 34:
i2c_write_byte(255);
break;
case 35:
UltrasonicRange = TWDR << 8;/* Read I2C Value */
UltrasonicRangeHigh = TWDR;
i2c_stop();
i2c_start();
break;
case 36:
i2c_write_byte(0xE0);
break;
case 37:
i2c_write_byte(0x03);
break;
case 38:
i2c_start();
break;
case 39:
i2c_write_byte(0xE1);
break;
case 40:
i2c_write_byte(UltrasonicRangeLow);
break;
case 41:
UltrasonicRange += TWDR;
UltrasonicRangeLow = TWDR;
i2c_stop();
I2CTimeout = 22;
twi_state = 0;
break;
}
#endif
TWCR |= 0x80;
}

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/twimaster.c - Rev 883