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; |