Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

add rotational transition matrix in euler

Browse Source
This commit is contained in:
Gautier Hattenberger
2010-05-04 08:43:56 +00:00
parent a79cea8331
commit 7e511cdef7
1 changed files with 43 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_int.h
+43
View File
@@ -716,8 +716,51 @@ struct Int64Vect3 {
#define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
int32_t cphi; \
PPRZ_ITRIG_COS(cphi, (_e).phi); \
int32_t stheta; \
PPRZ_ITRIG_SIN(stheta, (_e).theta); \
int32_t ctheta; \
PPRZ_ITRIG_COS(ctheta, (_e).theta); \
\
int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
\
(_r).p = - INT_MULT_RSHIFT(sphi, (_ed).psi, INT32_TRIG_FRAC) + (_ed).phi; \
(_r).q = INT_MULT_RSHIFT(sphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) + INT_MULT_RSHIFT(cphi, (_ed).theta, INT32_TRIG_FRAC); \
(_r).r = INT_MULT_RSHIFT(cphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_ed).theta, INT32_TRIG_FRAC); \
\
}
#define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed) INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)
#define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) { \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
int32_t cphi; \
PPRZ_ITRIG_COS(cphi, (_e).phi); \
int32_t stheta; \
PPRZ_ITRIG_SIN(stheta, (_e).theta); \
int64_t ctheta; \
PPRZ_ITRIG_COS(ctheta, (_e).theta); \
\
if (ctheta != 0) { \
int64_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
int64_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
\
(_ed).phi = (_r).p + (int32_t)((sphi_stheta * (int64_t)(_r).q) / ctheta) + (int32_t)((cphi_stheta * (int64_t)(_r).r) / ctheta); \
(_ed).theta = INT_MULT_RSHIFT(cphi, (_r).q, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_r).r, INT32_TRIG_FRAC); \
(_ed).psi = (int32_t)(((int64_t)sphi * (int64_t)(_r).q) / ctheta) + (int32_t)(((int64_t)cphi * (int64_t)(_r).r) / ctheta); \
} \
\
}
#define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r) INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)
/*
*