Rev 966 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 966 | Rev 973 | ||
---|---|---|---|
Line 619... | Line 619... | ||
619 | 619 | ||
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
620 | //matSetFull(matB, _Psi, _p, 0.0F); |
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
621 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
Line 622... | Line 622... | ||
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
622 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
623 | 623 | ||
624 | ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime; |
624 | ADC_p = -(float) AverageRoll * 0.00001F * fCycleTime; |
Line 625... | Line 625... | ||
625 | ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime; |
625 | ADC_q = -(float) AverageNick * 0.00001F * fCycleTime; |
626 | ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime; |
626 | ADC_r = -(float) AverageGier * 0.00001F * fCycleTime; |
627 | 627 |