| 621,9 → 621,9 |
|---|
| matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
| matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
| |
| ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime; |
| ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime; |
| ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime; |
| ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
| ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
| ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
| |
| fSumGier += ADC_r * 180.F / 3.14F; |
| fSumNick += ADC_q * 180.F / 3.14F; |