Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[rotorcraft] stabilization ref_quat_int infinitesimal integration should be correct now

Browse Source
This commit is contained in:
Felix Ruess
2012-11-28 15:24:51 +01:00
parent 7d886a29bc
commit da20a67bdc
2 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+7 -3
View File
@@ -118,6 +118,8 @@ void stabilization_attitude_ref_enter()
* Reference
*/
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
// CAUTION! Periodic frequency is assumed to be 512 Hz
// which is equal to >> 9
#define F_UPDATE_RES 9
void stabilization_attitude_ref_update() {
@@ -125,9 +127,11 @@ void stabilization_attitude_ref_update() {
/* integrate reference attitude */
struct Int32Quat qdot;
INT32_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
//QUAT_SMUL(qdot, qdot, RATE_BFP_OF_REAL(DT_UPDATE));
QUAT_SMUL(qdot, qdot, 4);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
//QUAT_SMUL(qdot, qdot, DT_UPDATE);
qdot.qi = qdot.qi >> F_UPDATE_RES;
qdot.qx = qdot.qx >> F_UPDATE_RES;
qdot.qy = qdot.qy >> F_UPDATE_RES;
qdot.qz = qdot.qz >> F_UPDATE_RES;
QUAT_ADD(stab_att_ref_quat, qdot);
INT32_QUAT_NORMALIZE(stab_att_ref_quat);
sw/airborne/math/pprz_algebra_int.h
+5 -4
View File
@@ -605,11 +605,12 @@ struct Int64Vect3 {
/* _qd = -0.5*omega(_r) * _q */
// mult with 0.5 is done by shifting one more bit to the right
#define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \
(_qd).qi = (-1*( (_r).p*(_q).qx/2 + (_r).q*(_q).qy + (_r).r*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qx = (-1*(-(_r).p*(_q).qi/2 - (_r).r*(_q).qy + (_r).q*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qy = (-1*(-(_r).q*(_q).qi/2 + (_r).r*(_q).qx - (_r).p*(_q).qz))>>INT32_RATE_FRAC; \
(_qd).qz = (-1*(-(_r).r*(_q).qi/2 - (_r).q*(_q).qx + (_r).p*(_q).qy))>>INT32_RATE_FRAC; \
(_qd).qi = (-( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz))>>(INT32_RATE_FRAC+1); \
(_qd).qx = (-(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz))>>(INT32_RATE_FRAC+1); \
(_qd).qy = (-(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz))>>(INT32_RATE_FRAC+1); \
(_qd).qz = (-(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy))>>(INT32_RATE_FRAC+1); \
}
/** in place quaternion first order integration with constant rotational velocity. */