| 178,10 → 178,11 |
|---|
| 17 L |
| */ |
| |
| long my_ACC_AltitudeFusion(unsigned char init) |
| long ACC_AltitudeFusion(unsigned char init) |
| { |
| static long HoeheAlt, HoehenDiff, VerticalVelocitySum, AccVertical, VerticalVelocity; |
| signed int tmpK; |
| |
| if(init) // beim Sensor Kalibrieren ist das 2 und beim Messbereich Erweitern ist das 1 |
| { |
| if(init == 2) VerticalVelocitySum = 0; |
| 191,10 → 192,17 |
|---|
| else |
| { |
| HoehenDiff = HoehenWert - HoeheAlt; |
| tmpK = AdWertAccRoll/32; |
| AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172 + tmpK*tmpK; |
| tmpK = AdWertAccNick/32; |
| AccVertical += tmpK*tmpK; |
| // -------------------- |
| // seit 2.00b |
| tmpK = AdWertAccRoll / 16; |
| // seit 2.00c |
| tmpK = (tmpK * abs(ToNaviCtrl.IntegralRoll)) / 512; // das soll sinus(roll) nachbilden |
| DebugOut.Analog[16] = tmpK; |
| AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172 + tmpK*tmpK; |
| tmpK = AdWertAccNick / 16; |
| tmpK = (tmpK * abs(ToNaviCtrl.IntegralNick)) / 512; // das soll sinus(nick) nachbilden |
| AccVertical += tmpK*tmpK; |
| // -------------------- |
| VerticalVelocitySum = VerticalVelocitySum - VerticalVelocity + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s |
| VerticalVelocity = (VerticalVelocitySum + 512)/1024; // cm/s |
| HoeheAlt = HoehenWert; |