Subversion Repositories FlightCtrl

#include "main.h"

volatile unsigned int CountMilliseconds = 0;
volatile static unsigned int tim_main;
volatile unsigned char UpdateMotor = 0;
volatile unsigned int cntKompass = 0;
volatile unsigned int beeptime = 0;
volatile unsigned char SendSPI = 0;
volatile unsigned int ServoState = 40;

unsigned int BeepMuster = 0xffff;
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
};


SIGNAL (SIG_OVERFLOW0)    // 8kHz
{
    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++;
    }  

     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(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)
{
    tim_main = SetDelay(10);
    TCCR0B = CK8;
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
    OCR0A =  0;
    OCR0B = 120;
    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);

    TIMSK0 |= _BV(TOIE0);
    OCR2A = 10;
    TCNT2 = 0;
   
}

// -----------------------------------------------------------------------

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);
}

// -----------------------------------------------------------------------
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;
}

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
//  Servo ansteuern
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
SIGNAL(SIG_OVERFLOW2) {

        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;

                // 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;
               
                if( EE_Parameter.ServoNickCompInvert & 0x01 )
                        ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) >> DIV_128 ) / ( 512L >> DIV_4 );
                else
                        ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) >> 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--;
}