Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 838 → Rev 966

/branches/KalmanFilter MikeW/analog.c
13,20 → 13,21
*/
 
#include "main.h"
#include "KalmanFc.h"
#include "FlightControl.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 UBat = 100; /* Initial Battery Guess */
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;
unsigned int AdWertAirPressure_Raw = 1023;
 
/* ****************************************************************************
Functionname: ADC_Init */ /*!
39,9 → 40,12
**************************************************************************** */
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
#ifdef INTERNAL_REFERENCE
ADMUX = 64;/* Internal Reference 5V */
#else
ADMUX = 0; /* External Reference */
#endif
ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
}
 
/* ****************************************************************************
55,21 → 59,21
**************************************************************************** */
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);
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);
}
 
/* ****************************************************************************
83,68 → 87,76
**************************************************************************** */
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;
static unsigned char kanal=0,state = 0;
ANALOG_OFF;
switch(state++)
{
case 0:
AdWertGier = ADC;
AdWertGier_Raw = ADC;
AccumulatedRoll_Z += (ADC - AdNeutralGier);
AccumulatedRoll_Z_cnt++;
kanal = 1;
break;
case 1:
AdWertRoll = ADC;
AdWertRoll_Raw = ADC;
AccumulatedRoll_X += (ADC - AdNeutralRoll);
AccumulatedRoll_X_cnt++;
kanal = 2;
break;
case 2:
AdWertNick = ADC;
AdWertNick_Raw = ADC;
AccumulatedRoll_Y += (ADC - AdNeutralNick);
AccumulatedRoll_Y_cnt++;
kanal = 4;
break;
case 3:
#ifdef INTERNAL_REFERENCE
UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */
#else
UBat = (3 * UBat + ADC / 3) / 4;
#endif
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;
#ifdef INTERNAL_REFERENCE
/* Add 64 in order to use the internal 5v refenrence */
ADMUX += 64;
#endif
ANALOG_ON;
}