Subversion Repositories FlightCtrl

/*
Copyright 2008, by Michael Walter

All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/

#include "main.h"
#include "KalmanFc.h"

int  UBat = 100;
int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
int  AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
int  AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;

int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0;
int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0;

unsigned int  AdWertAirPressure_Raw = 1023;

/* ****************************************************************************
Functionname:     ADC_Init                      */ /*!
Description:

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void ADC_Init(void)
{
    ADMUX = 0;//Referenz ist extern
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
    //Free Running Mode, Division Factor 128, Interrupt on
}

/* ****************************************************************************
Functionname:     SucheLuftruckOffset                      */ /*!
Description:

  @return           void
  @pre              -
  @post             -
  @author           H. Buss / I. Busker
**************************************************************************** */
void SucheLuftruckOffset(void)
{
        unsigned int off;
        off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
        if(off > 20) off -= 10;
        OCR0A = off;
        Delay_ms_Mess(100);
        if(AdWertAirPressure_Raw < 850) off = 0;
        for(; off < 250;off++)
        {
                OCR0A = off;
                Delay_ms_Mess(50);
                printf(".");  
                if(AdWertAirPressure_Raw < 900) break;
        }
        eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
        Delay_ms_Mess(300);
}

/* ****************************************************************************
Functionname:     SIGNAL                      */ /*!
Description:

  @return           void
  @pre              -
  @post             -
  @author           Michael Walter
**************************************************************************** */
SIGNAL(SIG_ADC)
{
    static unsigned char kanal=0,state = 0;
    ANALOG_OFF;
    switch(state++)
        {
        case 0:
                AdWertGier = ADC;
                AdWertGier_Raw = AdWertGier;
                AccumulatedRoll_Z += (ADC - AdNeutralGier);
                AccumulatedRoll_Z_cnt++;
                kanal = 1;
                break; 
        case 1:
                AdWertRoll = ADC;
                AdWertRoll_Raw = AdWertRoll;
                AccumulatedRoll_X += (ADC - AdNeutralRoll);
                AccumulatedRoll_X_cnt++;
                kanal = 2;
                break;         
        case 2:
                AdWertNick = ADC;
                AdWertNick_Raw = AdWertNick;
                AccumulatedRoll_Y += (ADC - AdNeutralNick);
                AccumulatedRoll_Y_cnt++;
                kanal = 4;
                break;                 
        case 3:
                UBat = (3 * UBat + ADC / 3) / 4;
                kanal = 6;
                break;
        case 4:
                AdWertAccRoll = NeutralAccY - ADC;
                AdWertAccRoll_Raw = ADC;
                AccumulatedACC_Y += (NeutralAccY - ADC);
                AccumulatedACC_Y_cnt++;
                kanal = 7;
                break;
        case 5:
                AdWertAccNick = ADC - NeutralAccX;
                AdWertAccNick_Raw = ADC;
                AccumulatedACC_X += (ADC - NeutralAccX);
                AccumulatedACC_X_cnt++;
                kanal = 5;
                break;
        case 6:
                AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
                AdWertAccHoch_Raw = ADC;
                AccumulatedACC_Z += (ADC - NeutralAccZ);
                AccumulatedACC_Z_cnt++;
                kanal = 3;
                break;
        case 7:
        AdWertAirPressure_Raw = ADC;
                AccumulatedAirPressure += ADC;
                AccumulatedAirPressure_cnt++;
                kanal = 0;
                state = 0;
                break;         
        default:
                kanal = 0;
                state = 0;
                break;
        }
    ADMUX = kanal;
        ANALOG_ON;
}