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

Add a few new quaternion macros to pprz_algebra_int (not yet tested)

Browse Source
This commit is contained in:
Allen
2011-05-23 10:55:48 -07:00
parent ff32873da0
commit d4a7fb0add
1 changed files with 26 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_int.h
+26
View File
@@ -579,8 +579,22 @@ struct Int64Vect3 {
(_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
INT32_QUAT_COMP(_a2c, _a2b, _b2c); \
INT32_QUAT_WRAP_SHORTEST(_a2c); \
INT32_QUAT_NORMALIZE(_a2c); \
}
/* _qd = -0.5*omega(_r) * _q */
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \
(_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
(_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy); \
}
#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
#define INT32_QUAT_VMULT(v_out, q, v_in) { \
const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \
@@ -662,6 +676,18 @@ struct Int64Vect3 {
INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
}
#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
int32_t san2; \
PPRZ_ITRIG_SIN(san2, (_an/2)); \
int32_t can2; \
PPRZ_ITRIG_SIN(can2, (_an/2)); \
_q.qi = can2; \
_q.qx = san2 * _uv.x; \
_q.qy = san2 * _uv.y; \
_q.qz = san2 * _uv.z; \
}
#define INT32_QUAT_OF_RMAT(_q, _r) { \
const int32_t tr = RMAT_TRACE(_r); \