Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "main.h"

volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
volatile int  AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0;
volatile int  accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 0;
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;
volatile long Luftdruck = 32000;
volatile int  StartLuftdruck;
volatile unsigned int  MessLuftdruck = 1023;
unsigned char DruckOffsetSetting;
volatile int HoeheD = 0;
volatile char messanzahl_Druck;
volatile int  tmpLuftdruck;
volatile unsigned int ZaehlMessungen = 0;

//#######################################################################################
//
void ADC_Init(void)
//#######################################################################################
{
    ADMUX = 0;//Referenz ist extern
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
    //Free Running Mode, Division Factor 128, Interrupt on
}

void SucheLuftruckOffset(void)
{
 unsigned int off;
 for(off=0; off < 250;off++)
  {
  OCR0A = off;
  Delay_ms(50);
  printf(".");  
  if(MessLuftdruck < 900) break;
  }
   DruckOffsetSetting = off;
   Delay_ms(200);
}


//#######################################################################################
//
SIGNAL(SIG_ADC)
//#######################################################################################
{
    static unsigned char kanal=0,state = 0;
    signed int wert;   
    ANALOG_OFF;
    switch(state++)
        {
        case 0:
            wert = (signed int) AdNeutralGier - ADC;
            AccumulateGier += wert;     //
            MessanzahlGier++;
            Mess_Integral_Gier += wert;// / 16;
            Mess_Integral_Gier2 += wert;
            kanal = 1;
            ZaehlMessungen++;
            break;
        case 1:
            wert = (signed int) ADC - AdNeutralRoll;
            Mess_IntegralRoll += wert;
            Mess_IntegralRoll2 += wert;
            if(ADC < 10)   wert = -700;
            if(ADC > 1000) wert = +700;
            AccumulateRoll += wert;
            MessanzahlRoll++;
            kanal = 2;
            break;
        case 2:
            wert = (signed int) ADC - AdNeutralNick;
            Mess_IntegralNick += wert;
            Mess_IntegralNick2 += wert;
            if(ADC < 10)   wert = -700;
            if(ADC > 1000) wert = +700;
            AccumulateNick += wert;
            MessanzahlNick++;
            kanal = 4;
            break;
        case 3:
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
            kanal = 6;
            break;
        case 4:
            Aktuell_ay = acc_neutral.Y - ADC;
            accumulate_AccRoll += Aktuell_ay;
            messanzahl_AccRoll++;
            kanal = 7;
            break;
        case 5:
            Aktuell_ax = ADC - acc_neutral.X;
            accumulate_AccNick +=  Aktuell_ax;
            messanzahl_AccNick++;
                    kanal = 5;
                    state = 6;
            break;
        case 6:
            accumulate_AccHoch =  (signed int) ADC - acc_neutral.Z;
            accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
            if(accumulate_AccHoch > 1)
             {
              if(acc_neutral.Z < 800) acc_neutral.Z+= 0.02;
             }  
             else if(accumulate_AccHoch < -1)
             {
              if(acc_neutral.Z > 600) acc_neutral.Z-= 0.02;
             }
            messanzahl_AccHoch = 1;
            Aktuell_az = ADC;
            Mess_Integral_Hoch += accumulate_AccHoch;      // Integrieren
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
//            Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen
 /*           if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
             {
                      kanal = 3;
                      state = 7;
             }
            else  
             {
                      kanal = 0;
                      state = 0;
             }*/
                      kanal = 3;
                      state = 7;
            break;
        case 7:
            tmpLuftdruck += ADC;
            if(++messanzahl_Druck >= 5)
                {
                MessLuftdruck = ADC;
                messanzahl_Druck = 0;
                                HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert);  // D-Anteil = neuerWert - AlterWert
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
                HoehenWert = StartLuftdruck - Luftdruck;
                tmpLuftdruck = 0;
                }
            kanal = 0;
            state = 0;
            break;
        default:
            kanal = 0;
            state = 0;
            break;
        }
    ADMUX = kanal;
    ANALOG_ON;
}