Subversion Repositories FlightCtrl

#include "main.h"

#include "parameter.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;

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

}