Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 838 → Rev 839

/branches/KalmanFilter MikeW/fc.c
File deleted
/branches/KalmanFilter MikeW/Bob4_OSD.c
77,8 → 77,8
**************************************************************************** */
void OsdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) OSDBuff[i] = ' ';
unsigned char i;
for(i=0;i<80;i++) OSDBuff[i] = ' ';
}
 
/* ****************************************************************************
94,8 → 94,8
**************************************************************************** */
void LcdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
unsigned char i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
}
 
 
162,21 → 162,21
**************************************************************************** */
SIGNAL(SIG_USART1_TRANS)
{
static unsigned int ptr = 0;
unsigned char tmp_tx;
if(!UART2PrintAbgeschlossen)
{
ptr++; // die [0] wurde schon gesendet
tmp_tx = OSDBuff[ptr];
if((tmp_tx == '\r') /* tmp_tx == 0 */)
{
ptr = 0;
UART2PrintAbgeschlossen = 1;
}
UDR1 = tmp_tx;
}
else ptr = 0;
static unsigned int ptr = 0;
unsigned char tmp_tx;
if(!UART2PrintAbgeschlossen)
{
ptr++; // die [0] wurde schon gesendet
tmp_tx = OSDBuff[ptr];
if((tmp_tx == '\r') /* tmp_tx == 0 */)
{
ptr = 0;
UART2PrintAbgeschlossen = 1;
}
UDR1 = tmp_tx;
}
else ptr = 0;
}
 
 
281,11 → 281,11
Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
dx = c_cos_8192(Phi45) / 128;
dy = c_sin_8192(Phi45) / 128;
x1 = 180 - dx;
x1 = 180 - dx;
y1 = 120 + 2 * Theta45 + dy;
x2 = 180 + dx;
y2 = y1 - 2 * dy;
OSDPtr = 0;
OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
#endif
/branches/KalmanFilter MikeW/twimaster.c
18,8 → 18,8
void i2c_init(void)
//############################################################################
{
TWSR = 0;
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
TWSR = 0;
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
}
 
//############################################################################
42,19 → 42,19
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);
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);
}
 
//############################################################################
79,349 → 79,349
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:
{
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];
}
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:
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];
}
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:
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];
}
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:
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];
}
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;
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
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;
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
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:
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;
}
{
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;
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();
#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();
}
 
if (TriggerSonicReading == 1)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 20;
break;
}
else if (TriggerSonicReading == 190)
{
// Get Ultrasonic Reading
i2c_start();
}
else if (TriggerSonicReading == 190)
{
// Get Ultrasonic Reading
i2c_start();
twi_state = 30;
break;
}
else
{
I2CTimeout = 10;
twi_state = 0;
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;
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;
TWCR |= 0x80;
}