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