| 70,6 → 70,7 |
|---|
| volatile unsigned char AdReady = 1; |
| volatile long HoehenWertF = 0, HoehenDiff, HoeheAlt; |
| volatile long vv, vvSum; |
| signed int AccZSum = 0; |
| |
| //####################################################################################### |
| void ADC_Init(void) |
| 238,13 → 239,13 |
|---|
| break; |
| case 8: |
| { |
| signed int tmp; |
| // signed int tmp; |
| Aktuell_az = ADC; |
| //if(PlatinenVersion < 22) |
| //else Aktuell_az = 1024 - ADC; |
| tmp = (signed int) Aktuell_az - NeutralAccZ; |
| AdWertAccHoch -= AdWertAccHoch/32; |
| AdWertAccHoch += tmp; |
| // tmp = (signed int) Aktuell_az - NeutralAccZ; |
| AccZSum = AccZSum - AdWertAccHoch + Aktuell_az - NeutralAccZ; |
| AdWertAccHoch = AccZSum/32; |
| // AdWertAccHoch = (7 * AdWertAccHoch + tmp) / 8; |
| } |
| if(PlatinenVersion < 22) |
| 322,7 → 323,7 |
|---|
| break; |
| // - neu ------------------------------------------------- |
| case 17: |
| J17_ON; //Qopter: testweise zur Rechenzeitmessung |
| J17_ON; //Qopter: testweise zur Rechenzeitmessung |
| HoehenDiff = HoehenWert - HoeheAlt; |
| // vvSum = vvSum - vv + ((Aktuell_az - NeutralAccZ) - AccShorter)*7 + HoehenDiff*500; // Fusion vert. velocity |
| vvSum = vvSum - vv + (Aktuell_az - NeutralAccZ)*7 + HoehenDiff*500; // Fusion vert. velocity |
| 330,7 → 331,7 |
|---|
| HoeheAlt = HoehenWert; |
| SummenHoehe = SummenHoehe - HoehenWertF + vv + HoehenWert; //Fusion Hoehe |
| HoehenWertF = (SummenHoehe + SM_FILTER/2)/SM_FILTER; // cm |
| J17_OFF; |
| J17_OFF; |
| DebugOut.Analog[19] = (Aktuell_az - NeutralAccZ); |
| //DebugOut.Analog[16] = ((Aktuell_az - NeutralAccZ) - AccShorter); |
| state = 0; |
| 343,7 → 344,9 |
|---|
| Luftdruck -= Luftdruck/16; |
| Luftdruck += tmpLuftdruck; |
| HoehenWert = (StartLuftdruck - Luftdruck)/1; // cm |
| if(PlatinenVersion > 21) HoehenWert = HoehenWert/3 + AdWertAccHoch/200; //+ compensates disturbance due to acceleration |
| J17_ON; //Qopter: testweise zur Rechenzeitmessung |
| if(PlatinenVersion > 21) HoehenWert = HoehenWert/3 + AdWertAccHoch*41/256; //+ compensates disturbance due to acceleration |
| J17_OFF; |
| /* // Qopter: deaktivieren Anfang |
| if(PlatinenVersion > 21 && NeutralAccZ) MessLuftdruck -= (Aktuell_az - NeutralAccZ)/12; |
| tmpLuftdruck += MessLuftdruck; |