| 69,7 → 69,7 |
|---|
| unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115; |
| volatile unsigned char AdReady = 1; |
| volatile long HoehenWertF = 0, HoehenDiff, HoeheAlt; |
| volatile long vv, vvSum; |
| volatile long vv, vvSum, AccVertical; |
| //signed int AccZSum = 0; |
| |
| //####################################################################################### |
| 328,9 → 328,19 |
|---|
| case 17: |
| HoehenDiff = HoehenWert - HoeheAlt; |
| // vvSum = vvSum - vv + ((Aktuell_az - NeutralAccZ) - AccShorter)*7 + HoehenDiff*500; // Fusion vert. velocity |
| vvSum = vvSum - vv + AdWertAccHoch*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s |
| AltLoss = HoehenWertF - HoehenWert; |
| if((AltLoss > 50) && (Parameter_UserParam1 > 50)) vvSum -= AltLoss*2; // avoids height loss |
| J17_ON; //Qopter: testweise zur Rechenzeitmessung |
| if(Parameter_UserParam1 > 50) |
| { |
| AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172; |
| vvSum = vvSum - vv + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s |
| } |
| else |
| { |
| vvSum = vvSum - vv + AdWertAccHoch*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s |
| } |
| J17_OFF; |
| // AltLoss = HoehenWertF - HoehenWert; |
| // if((AltLoss > 50) && (Parameter_UserParam1 > 50)) vvSum -= AltLoss*2; // avoids height loss |
| vv = (vvSum + 512)/1024; // cm/s |
| HoeheAlt = HoehenWert; |
| SummenHoehe = SummenHoehe - HoehenWertF + vv + HoehenWert; //Fusion Hoehe |
| 345,10 → 355,8 |
|---|
| MessLuftdruck = ADC; |
| result[messanzahl_Druck] = MessLuftdruck - 523 * (long)ExpandBaro; // -523 counts per offset step |
| messanzahl_Druck = (messanzahl_Druck + 1)&(15); |
| //J17_ON; //Qopter: testweise zur Rechenzeitmessung |
| tmpLuftdruck = 0; |
| for(int j=0;j<16;j++) {tmpLuftdruck+=result[j];} |
| //J17_OFF; |
| // Luftdruck = tmpLuftdruck; |
| Luftdruck = (15 * Luftdruck + tmpLuftdruck) / 16; // noise reduction |
| HoehenWert = StartLuftdruck - Luftdruck; |