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