Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1126 → Rev 1127

/branches/thjac/V1_10/timer0.c
14,212 → 14,204
unsigned int ServoValue = 0;
 
enum {
STOP = 0,
CK = 1,
CK8 = 2,
CK64 = 3,
CK256 = 4,
CK1024 = 5,
T0_FALLING_EDGE = 6,
T0_RISING_EDGE = 7
STOP = 0,
CK = 1,
CK8 = 2,
CK64 = 3,
CK256 = 4,
CK1024 = 5,
T0_FALLING_EDGE = 6,
T0_RISING_EDGE = 7
};
 
 
SIGNAL (SIG_OVERFLOW0) // 8kHz
SIGNAL(SIG_OVERFLOW0) // 8kHz
{
static unsigned char cnt_1ms = 1,cnt = 0;
static unsigned char cnt_1ms = 1, cnt = 0;
unsigned char pieper_ein = 0;
// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
if(SendSPI) SendSPI--;
if(!cnt--)
{
cnt = 9;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms) UpdateMotor = 1;
CountMilliseconds++;
}
// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
if (SendSPI)
SendSPI--;
if (!cnt--) {
// cnt = 9; // Wenn der Kommentar 8kHz oben stimmt, muß durch 8 geteilt werden, nicht durch 10!
cnt = 7;
// cnt_1ms++;
// cnt_1ms %= 2;
cnt_1ms = !cnt_1ms; // So ist das etwas einfacher
if (!cnt_1ms)
UpdateMotor = 1;
CountMilliseconds++;
}
 
if(beeptime > 1)
{
beeptime--;
if(beeptime & BeepMuster)
{
pieper_ein = 1;
}
else pieper_ein = 0;
}
else
{
pieper_ein = 0;
BeepMuster = 0xffff;
}
if (beeptime > 1) {
beeptime--;
if (beeptime & BeepMuster)
pieper_ein = 1;
else
pieper_ein = 0;
} else {
pieper_ein = 0;
BeepMuster = 0xffff;
}
 
if (pieper_ein) {
if (PlatinenVersion == 10)
PORTD |= (1 << 2); // Speaker an PORTD.2
else
PORTC |= (1 << 7); // Speaker an PORTC.7
} else {
if (PlatinenVersion == 10)
PORTD &= ~(1 << 2);
else
PORTC &= ~(1 << 7);
}
 
if(pieper_ein)
{
if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
else PORTC |= (1<<7); // Speaker an PORTC.7
if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) {
if (PINC & 0x10)
cntKompass++;
else {
if ((cntKompass) && (cntKompass < 362)) {
cntKompass += cntKompass / 41;
if (cntKompass > 10) KompassValue = cntKompass - 10;
else KompassValue = 0;
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 0;
}
else
{
if(PlatinenVersion == 10) PORTD &= ~(1<<2);
else PORTC &= ~(1<<7);
}
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
{
if(PINC & 0x10)
{
cntKompass++;
}
else
{
if((cntKompass) && (cntKompass < 362))
{
cntKompass += cntKompass / 41;
if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
}
// if(cntKompass < 10) cntKompass = 10;
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 0;
}
}
}
}
 
//----------------------------
void Timer_Init(void)
{
 
void Timer_Init(void) {
tim_main = SetDelay(10);
TCCR0B = CK8;
TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
OCR0A = 0;
TCCR0A = (1 << COM0A1) | (1 << COM0B1) | 3; //fast PWM
OCR0A = 0;
OCR0B = 120;
TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
TCNT0 = (unsigned char) - TIMER_RELOAD_VALUE; // reload
//OCR1 = 0x00;
 
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
// TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256
TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22); // clk/64
TIMSK2 |= _BV(OCIE2A);
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3;
// TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22); // clk/256
TCCR2B = (0 << CS20) | (0 << CS21) | (1 << CS22); // clk/64
 
 
TIMSK2 |= _BV(OCIE2A);
 
TIMSK0 |= _BV(TOIE0);
OCR2A = 10;
TCNT2 = 0;
 
}
 
// -----------------------------------------------------------------------
 
unsigned int SetDelay (unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(CountMilliseconds + t + 1);
// TIMSK0 |= _BV(TOIE0);
unsigned int SetDelay(unsigned int t) {
// TIMSK0 &= ~_BV(TOIE0);
return (CountMilliseconds + t + 1);
// TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
char CheckDelay(unsigned int t)
{
// TIMSK0 &= ~_BV(TOIE0);
return(((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= _BV(TOIE0);
 
char CheckDelay(unsigned int t) {
// TIMSK0 &= ~_BV(TOIE0);
return (((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= _BV(TOIE0);
}
 
// -----------------------------------------------------------------------
void Delay_ms(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
 
void Delay_ms(unsigned int w) {
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
}
 
void Delay_ms_Mess(unsigned int w)
{
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt)) ANALOG_ON;
void Delay_ms_Mess(unsigned int w) {
unsigned int akt;
akt = SetDelay(w);
while (!CheckDelay(akt))
ANALOG_ON;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Servo ansteuern
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
SIGNAL(SIG_OVERFLOW2) {
 
if (ServoState > 0)
PORTD |= 0x80;
else
PORTD &= ~0x80;
TCCR2A = 3;
TIMSK2 &= ~_BV( TOIE2 );
if (ServoState > 0)
PORTD |= 0x80;
else
PORTD &= ~0x80;
 
TCCR2A = 3;
TIMSK2 &= ~_BV(TOIE2);
}
 
SIGNAL(SIG_OUTPUT_COMPARE2A) {
 
static unsigned char postPulse = 0x80;
static int filterServo = 100;
if( ServoState == 4 ) {
ServoValue = 0x0030; // Offset Part1
filterServo = ( filterServo * 3 + (int) Parameter_ServoNickControl * 2 ) >> DIV_4;
ServoValue += filterServo;
static unsigned char postPulse = 0x80;
static int filterServo = 100;
 
// Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden
if( ServoValue < ( (int) EE_Parameter.ServoNickMin * 3 ) )
ServoValue = (int) EE_Parameter.ServoNickMin * 3;
else if( ServoValue > ( (int) EE_Parameter.ServoNickMax * 3 ) )
ServoValue = (int) EE_Parameter.ServoNickMax * 3;
long integral;
if (ServoState == 4) {
 
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if( PARAM_X_FORMATION ) {
integral = IntegralNick - IntegralRoll;
} else {
integral = IntegralNick;
}
ServoValue = 0x0030; // Offset Part1
filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2) >> DIV_4;
ServoValue += filterServo;
 
if( EE_Parameter.ServoNickCompInvert & 0x01 )
ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
else
ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
DebugOut.Analog[20] = ServoValue;
if ( ( ServoValue % 255 ) < 45 ) {
ServoValue += 77;
postPulse = 0x60 - 77;
} else {
postPulse = 0x60;
}
OCR2A = 255 - ( ServoValue % 256 );
TCCR2A = (1 << COM2A1 ) | ( 1 << COM2A0 ) | 3;
// Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden
if (ServoValue < ((int) EE_Parameter.ServoNickMin * 3))
ServoValue = (int) EE_Parameter.ServoNickMin * 3;
else if (ServoValue > ((int) EE_Parameter.ServoNickMax * 3))
ServoValue = (int) EE_Parameter.ServoNickMax * 3;
 
} else if( ( ServoState > 0 ) && ( ServoState < 4 ) ) {
if( ServoValue > 255 ) {
PORTD |= 0x80;
TCCR2A = 3;
ServoValue -= 255;
} else {
TCCR2A = ( 1 << COM2A1 ) | ( 0 << COM2A0 ) | 3;
OCR2A = postPulse; // Offset Part2
ServoState = 1;
}
} else if( ServoState == 0 ) {
ServoState = (int) EE_Parameter.ServoNickRefresh << MUL_4;
PORTD &= ~0x80;
TCCR2A = 3;
}
ServoState--;
long integral;
 
/* Über Parameter läßt sich zwischen "+" und "X" - Formations
* umschalten (sh. parameter.h)
*/
if (PARAM_X_FORMATION)
integral = IntegralNick - IntegralRoll;
else
integral = IntegralNick;
 
if (EE_Parameter.ServoNickCompInvert & 0x01)
ServoValue += ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4);
else
ServoValue -= ((long) ((long) EE_Parameter.ServoNickComp * integral) >> DIV_128) / (512L >> DIV_4);
 
DebugOut.Analog[20] = ServoValue;
 
if ((ServoValue % 255) < 45) {
ServoValue += 77;
postPulse = 0x60 - 77;
} else
postPulse = 0x60;
 
OCR2A = 255 - (ServoValue % 256);
TCCR2A = (1 << COM2A1) | (1 << COM2A0) | 3;
 
} else if ((ServoState > 0) && (ServoState < 4)) {
 
if (ServoValue > 255) {
PORTD |= 0x80;
TCCR2A = 3;
ServoValue -= 255;
} else {
TCCR2A = (1 << COM2A1) | (0 << COM2A0) | 3;
OCR2A = postPulse; // Offset Part2
ServoState = 1;
}
 
} else if (ServoState == 0) {
ServoState = (int) EE_Parameter.ServoNickRefresh << MUL_4;
PORTD &= ~0x80;
TCCR2A = 3;
}
 
ServoState--;
}