192,8 → 192,7 |
static signed int gier1, roll1, nick1, nick_filter, roll_filter; |
static signed int accy, accx; |
static long tmpLuftdruck = 0; |
static long result[16]; |
static unsigned char messanzahl_Druck = 0; |
// static char messanzahl_Druck = 0; |
#define ANZ_RING 32 |
//static long VarioRing[ANZ_RING]; |
//static unsigned char RingPtr = 0; |
246,9 → 245,6 |
//else Aktuell_az = 1024 - ADC; |
// tmp = (signed int) Aktuell_az - NeutralAccZ; |
AdWertAccHoch = Aktuell_az - NeutralAccZ; |
// AccZSum = AccZSum - AdWertAccHoch + Aktuell_az - NeutralAccZ; |
// AdWertAccHoch = AccZSum/32; |
// AdWertAccHoch = (7 * AdWertAccHoch + tmp) / 8; |
} |
if(PlatinenVersion < 22) |
{ |
326,7 → 322,6 |
// - neu ------------------------------------------------- |
case 17: |
HoehenDiff = HoehenWert - HoeheAlt; |
// vvSum = vvSum - vv + ((Aktuell_az - NeutralAccZ) - AccShorter)*7 + HoehenDiff*500; // Fusion vert. velocity |
AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172; |
vvSum = vvSum - vv + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s |
vv = (vvSum + 512)/1024; // cm/s |
333,8 → 328,7 |
HoeheAlt = HoehenWert; |
SummenHoehe = SummenHoehe - HoehenWertF + vv + HoehenWert; //Fusion Hoehe |
HoehenWertF = (SummenHoehe + SM_FILTER/2)/SM_FILTER; // cm |
//DebugOut.Analog[19] = AdWertAccHoch*7; |
//DebugOut.Analog[16] = ((Aktuell_az - NeutralAccZ) - AccShorter); |
|
state = 0; |
AdReady = 1; |
ZaehlMessungen++; |
341,17 → 335,12 |
// "break" fehlt hier absichtlich |
case 9: |
MessLuftdruck = ADC; |
result[messanzahl_Druck] = MessLuftdruck - 523 * (long)ExpandBaro; // -523 counts per offset step |
messanzahl_Druck = (messanzahl_Druck + 1)&(15); |
tmpLuftdruck = 0; |
for(int j=0;j<16;j++) {tmpLuftdruck+=result[j];} |
// Luftdruck = tmpLuftdruck; |
Luftdruck = (15 * Luftdruck + tmpLuftdruck) / 16; // noise reduction |
HoehenWert = StartLuftdruck - Luftdruck; |
tmpLuftdruck = MessLuftdruck - 523 * (long)ExpandBaro; // -523 counts per offset step |
Luftdruck -= Luftdruck/16; |
Luftdruck += tmpLuftdruck; |
HoehenWert = StartLuftdruck - Luftdruck; // cm |
|
if(PlatinenVersion > 21) HoehenWert = HoehenWert + AdWertAccHoch*41/256; |
// if (messanzahl_Druck == 0) OCR0A++; // generates sawtooth |
// if (messanzahl_Druck == 1) OCR0A--; |
// if(PlatinenVersion > 21) HoehenWert = HoehenWert*5/16 + AdWertAccHoch*41/256; //+ compensates disturbance due to acceleration |
/* // Qopter: deaktivieren Anfang |
if(PlatinenVersion > 21 && NeutralAccZ) MessLuftdruck -= (Aktuell_az - NeutralAccZ)/12; |
tmpLuftdruck += MessLuftdruck; |