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