diff --git a/conf/settings/control/stabilization_att_int_quat.xml b/conf/settings/control/stabilization_att_int_quat.xml
index b76196174b..fd84e02734 100644
--- a/conf/settings/control/stabilization_att_int_quat.xml
+++ b/conf/settings/control/stabilization_att_int_quat.xml
@@ -17,12 +17,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index 7cc023ccce..69935a21fe 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -60,6 +60,11 @@ struct Int32Quat stabilization_att_sum_err_quat;
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
+struct Int32Quat stab_att_sp_quat;
+struct Int32Eulers stab_att_sp_euler;
+
+struct AttRefQuatInt att_ref_quat_i;
+
#define IERROR_SCALE 128
#define GAIN_PRESCALER_FF 48
#define GAIN_PRESCALER_P 12
@@ -99,25 +104,25 @@ static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
- &stab_att_ref_euler.phi,
- &stab_att_ref_euler.theta,
- &stab_att_ref_euler.psi,
- &stab_att_ref_rate.p,
- &stab_att_ref_rate.q,
- &stab_att_ref_rate.r,
- &stab_att_ref_accel.p,
- &stab_att_ref_accel.q,
- &stab_att_ref_accel.r);
+ &att_ref_quat_i.euler.phi,
+ &att_ref_quat_i.euler.theta,
+ &att_ref_quat_i.euler.psi,
+ &att_ref_quat_i.rate.p,
+ &att_ref_quat_i.rate.q,
+ &att_ref_quat_i.rate.r,
+ &att_ref_quat_i.accel.p,
+ &att_ref_quat_i.accel.q,
+ &att_ref_quat_i.accel.r);
}
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
{
struct Int32Quat *quat = stateGetNedToBodyQuat_i();
pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
- &stab_att_ref_quat.qi,
- &stab_att_ref_quat.qx,
- &stab_att_ref_quat.qy,
- &stab_att_ref_quat.qz,
+ &att_ref_quat_i.quat.qi,
+ &att_ref_quat_i.quat.qx,
+ &att_ref_quat_i.quat.qy,
+ &att_ref_quat_i.quat.qz,
&(quat->qi),
&(quat->qx),
&(quat->qy),
@@ -128,7 +133,7 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d
void stabilization_attitude_init(void)
{
- stabilization_attitude_ref_init();
+ attitude_ref_quat_int_init(&att_ref_quat_i);
int32_quat_identity(&stabilization_att_sum_err_quat);
@@ -145,7 +150,7 @@ void stabilization_attitude_enter(void)
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_i();
- stabilization_attitude_ref_enter();
+ attitude_ref_quat_int_enter(&att_ref_quat_i, stab_att_sp_euler.psi);
int32_quat_identity(&stabilization_att_sum_err_quat);
@@ -224,8 +229,10 @@ void stabilization_attitude_run(bool_t enable_integrator)
/*
* Update reference
+ * Warning: dt is currently not used in the quat_int ref impl
*/
- stabilization_attitude_ref_update();
+ static const float dt = (1./PERIODIC_FREQUENCY);
+ attitude_ref_quat_int_update(&att_ref_quat_i, &stab_att_sp_quat, dt);
/*
* Compute errors for feedback
@@ -234,16 +241,16 @@ void stabilization_attitude_run(bool_t enable_integrator)
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
- INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
+ INT32_QUAT_INV_COMP(att_err, *att_quat, att_ref_quat_i.quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
/* rate error */
const struct Int32Rates rate_ref_scaled = {
- OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
- OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
- OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
+ OFFSET_AND_ROUND(att_ref_quat_i.rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(att_ref_quat_i.rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(att_ref_quat_i.rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Rates rate_err;
struct Int32Rates *body_rate = stateGetBodyRates_i();
@@ -264,7 +271,7 @@ void stabilization_attitude_run(bool_t enable_integrator)
}
/* compute the feed forward command */
- attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel);
+ attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &att_ref_quat_i.accel);
/* compute the feed back command */
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
index 84c954ac86..b9a05837cd 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
@@ -27,4 +27,30 @@
#include "math/pprz_algebra_int.h"
+extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
+extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
+
+extern struct AttRefQuatInt att_ref_quat_i;
+
+/* settings handlers for ref model params */
+#define stabilization_attitude_quat_int_SetOmegaP(_val) { \
+ attitude_ref_quat_int_set_omega_p(&att_ref_quat_i, _val); \
+ }
+#define stabilization_attitude_quat_int_SetOmegaQ(_val) { \
+ attitude_ref_quat_int_set_omega_q(&att_ref_quat_i, _val); \
+ }
+#define stabilization_attitude_quat_int_SetOmegaR(_val) { \
+ attitude_ref_quat_int_set_omega_r(&att_ref_quat_i, _val); \
+ }
+
+#define stabilization_attitude_quat_int_SetZetaP(_val) { \
+ attitude_ref_quat_int_set_zeta_p(&att_ref_quat_i, _val); \
+ }
+#define stabilization_attitude_quat_int_SetZetaQ(_val) { \
+ attitude_ref_quat_int_set_zeta_q(&att_ref_quat_i, _val); \
+ }
+#define stabilization_attitude_quat_int_SetZetaR(_val) { \
+ attitude_ref_quat_int_set_zeta_r(&att_ref_quat_i, _val); \
+ }
+
#endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
index b004b357cf..8459c40134 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
@@ -33,12 +33,11 @@
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
+#if 0
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC
extern struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC
-
-extern void stabilization_attitude_ref_init(void);
-extern void stabilization_attitude_ref_update(void);
+#endif
#endif /* STABILIZATION_ATTITUDE_REF_INT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
index abf5dcf762..e778657dd5 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -40,214 +40,218 @@
#define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC)
-struct Int32Eulers stab_att_sp_euler;
-struct Int32Quat stab_att_sp_quat;
-struct Int32Eulers stab_att_ref_euler;
-struct Int32Quat stab_att_ref_quat;
-struct Int32Rates stab_att_ref_rate;
-struct Int32Rates stab_att_ref_accel;
-
-struct FloatRefModel stab_att_ref_model = {
- {STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R},
- {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R}
-};
#define TWO_ZETA_OMEGA_RES 10
#define TWO_OMEGA_2_RES 7
-static struct Int32Rates two_zeta_omega;
-static struct Int32Rates two_omega_2;
-static void update_ref_model_p(void)
+
+static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi);
+static void update_ref_model_p(struct AttRefQuatInt *ref);
+static void update_ref_model_q(struct AttRefQuatInt *ref);
+static void update_ref_model_r(struct AttRefQuatInt *ref);
+static void update_ref_model(struct AttRefQuatInt *ref);
+
+/*
+ *
+ * Implementation.
+ * Should not rely on any global variables, so these functions can be used like a lib.
+ *
+ */
+void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
{
- two_zeta_omega.p = BFP_OF_REAL((2 * stab_att_ref_model.zeta.p * stab_att_ref_model.omega.p), TWO_ZETA_OMEGA_RES);
- two_omega_2.p = BFP_OF_REAL((2 * stab_att_ref_model.omega.p * stab_att_ref_model.omega.p), TWO_OMEGA_2_RES);
+ INT_EULERS_ZERO(ref->euler);
+ int32_quat_identity(&ref->quat);
+ INT_RATES_ZERO(ref->rate);
+ INT_RATES_ZERO(ref->accel);
+
+ struct FloatRates omega0 = {STABILIZATION_ATTITUDE_REF_OMEGA_P,
+ STABILIZATION_ATTITUDE_REF_OMEGA_Q,
+ STABILIZATION_ATTITUDE_REF_OMEGA_R};
+ struct FloatRates zeta0 = {STABILIZATION_ATTITUDE_REF_ZETA_P,
+ STABILIZATION_ATTITUDE_REF_ZETA_Q,
+ STABILIZATION_ATTITUDE_REF_ZETA_R};
+ attitude_ref_quat_int_set_omega(ref, &omega0);
+ attitude_ref_quat_int_set_zeta(ref, &zeta0);
+
+ /* calc the intermediate cached values */
+ update_ref_model(ref);
}
-static void update_ref_model_q(void)
+void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, int32_t psi)
{
- two_zeta_omega.q = BFP_OF_REAL((2 * stab_att_ref_model.zeta.q * stab_att_ref_model.omega.q), TWO_ZETA_OMEGA_RES);
- two_omega_2.q = BFP_OF_REAL((2 * stab_att_ref_model.omega.q * stab_att_ref_model.omega.q), TWO_OMEGA_2_RES);
-}
-
-static void update_ref_model_r(void)
-{
- two_zeta_omega.r = BFP_OF_REAL((2 * stab_att_ref_model.zeta.r * stab_att_ref_model.omega.r), TWO_ZETA_OMEGA_RES);
- two_omega_2.r = BFP_OF_REAL((2 * stab_att_ref_model.omega.r * stab_att_ref_model.omega.r), TWO_OMEGA_2_RES);
-}
-
-static void update_ref_model(void)
-{
- update_ref_model_p();
- update_ref_model_q();
- update_ref_model_r();
-}
-
-
-void stabilization_attitude_ref_set_omega_p(float omega_p)
-{
- stab_att_ref_model.omega.p = omega_p;
- update_ref_model_p();
-}
-
-void stabilization_attitude_ref_set_omega_q(float omega_q)
-{
- stab_att_ref_model.omega.q = omega_q;
- update_ref_model_q();
-}
-
-void stabilization_attitude_ref_set_omega_r(float omega_r)
-{
- stab_att_ref_model.omega.r = omega_r;
- update_ref_model_r();
-}
-
-void stabilization_attitude_ref_set_omega(struct FloatRates *omega)
-{
- stabilization_attitude_ref_set_omega_p(omega->p);
- stabilization_attitude_ref_set_omega_q(omega->q);
- stabilization_attitude_ref_set_omega_r(omega->r);
-}
-
-void stabilization_attitude_ref_set_zeta_p(float zeta_p)
-{
- stab_att_ref_model.zeta.p = zeta_p;
- update_ref_model_p();
-}
-
-void stabilization_attitude_ref_set_zeta_q(float zeta_q)
-{
- stab_att_ref_model.zeta.q = zeta_q;
- update_ref_model_q();
-}
-
-void stabilization_attitude_ref_set_zeta_r(float zeta_r)
-{
- stab_att_ref_model.zeta.r = zeta_r;
- update_ref_model_r();
-}
-
-void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta)
-{
- stabilization_attitude_ref_set_zeta_p(zeta->p);
- stabilization_attitude_ref_set_zeta_q(zeta->q);
- stabilization_attitude_ref_set_zeta_r(zeta->r);
-}
-
-
-static inline void reset_psi_ref_from_body(void)
-{
- //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
- stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
- stab_att_ref_rate.r = 0;
- stab_att_ref_accel.r = 0;
-}
-
-void stabilization_attitude_ref_init(void)
-{
-
- INT_EULERS_ZERO(stab_att_sp_euler);
- int32_quat_identity(&stab_att_sp_quat);
- INT_EULERS_ZERO(stab_att_ref_euler);
- int32_quat_identity(&stab_att_ref_quat);
- INT_RATES_ZERO(stab_att_ref_rate);
- INT_RATES_ZERO(stab_att_ref_accel);
-
- update_ref_model();
-
-}
-
-void stabilization_attitude_ref_enter(void)
-{
- reset_psi_ref_from_body();
+ reset_psi_ref(ref, psi);
/* convert reference attitude with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */
struct Int32Eulers ref_eul;
- INT32_EULERS_RSHIFT(ref_eul, stab_att_ref_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
- int32_quat_of_eulers(&stab_att_ref_quat, &ref_eul);
- int32_quat_wrap_shortest(&stab_att_ref_quat);
+ INT32_EULERS_RSHIFT(ref_eul, ref->euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
+ int32_quat_of_eulers(&ref->quat, &ref_eul);
+ int32_quat_wrap_shortest(&ref->quat);
/* set reference rate and acceleration to zero */
- memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
- memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
+ memset(&ref->accel, 0, sizeof(struct Int32Rates));
+ memset(&ref->rate, 0, sizeof(struct Int32Rates));
}
-/*
- * 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
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
-
-
-void stabilization_attitude_ref_update(void)
+/**
+ * Propagate reference.
+ * FIXME: use dt instead of hardcoded 512Hz via F_UPDATE_RES
+ */
+void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
{
-
/* integrate reference attitude */
const struct Int32Rates rate_ref_scaled = {
- OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
- OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
- OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
+ OFFSET_AND_ROUND(ref->rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(ref->rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(ref->rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Quat qdot;
- int32_quat_derivative(&qdot, &rate_ref_scaled, &stab_att_ref_quat);
- //QUAT_SMUL(qdot, qdot, DT_UPDATE);
+ int32_quat_derivative(&qdot, &rate_ref_scaled, &ref->quat);
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);
+ QUAT_ADD(ref->quat, qdot);
+ int32_quat_normalize(&ref->quat);
/* integrate reference rotational speeds
* delta rate = ref_accel * dt
* ref_rate = old_ref_rate + delta_rate
*/
const struct Int32Rates delta_rate = {
- stab_att_ref_accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
- stab_att_ref_accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
- stab_att_ref_accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)
+ ref->accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
+ ref->accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
+ ref->accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)
};
- RATES_ADD(stab_att_ref_rate, delta_rate);
+ RATES_ADD(ref->rate, delta_rate);
/* compute reference angular accelerations */
struct Int32Quat err;
/* compute reference attitude error */
- int32_quat_inv_comp(&err, &stab_att_sp_quat, &stab_att_ref_quat);
+ int32_quat_inv_comp(&err, sp_quat, &ref->quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&err);
/* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */
const struct Int32Rates accel_rate = {
- (-two_zeta_omega.p * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
- (-two_zeta_omega.q * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
- (-two_zeta_omega.r * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES)
+ (-ref->model.two_zeta_omega.p * (ref->rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
+ (-ref->model.two_zeta_omega.q * (ref->rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES),
+ (-ref->model.two_zeta_omega.r * (ref->rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES)
};
/* since error quaternion contains the half-angles we get 2*omega^2*err */
const struct Int32Rates accel_angle = {
- (-two_omega_2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
- (-two_omega_2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
- (-two_omega_2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES)
+ (-ref->model.two_omega2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
+ (-ref->model.two_omega2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES),
+ (-ref->model.two_omega2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES)
};
- RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
+ RATES_SUM(ref->accel, accel_rate, accel_angle);
/* saturate acceleration */
const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
- RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+ RATES_BOUND_BOX(ref->accel, MIN_ACCEL, MAX_ACCEL);
/* saturate angular speed and trim accel accordingly */
- SATURATE_SPEED_TRIM_ACCEL();
+ SATURATE_SPEED_TRIM_ACCEL(*ref);
/* compute ref_euler for debugging and telemetry */
struct Int32Eulers ref_eul;
- int32_eulers_of_quat(&ref_eul, &stab_att_ref_quat);
- INT32_EULERS_LSHIFT(stab_att_ref_euler, ref_eul, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
+ int32_eulers_of_quat(&ref_eul, &ref->quat);
+ INT32_EULERS_LSHIFT(ref->euler, ref_eul, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
+}
+
+
+
+
+static void update_ref_model_p(struct AttRefQuatInt *ref)
+{
+ ref->model.two_zeta_omega.p = BFP_OF_REAL((2 * ref->model.zeta.p * ref->model.omega.p), TWO_ZETA_OMEGA_RES);
+ ref->model.two_omega2.p = BFP_OF_REAL((2 * ref->model.omega.p * ref->model.omega.p), TWO_OMEGA_2_RES);
+}
+
+static void update_ref_model_q(struct AttRefQuatInt *ref)
+{
+ ref->model.two_zeta_omega.q = BFP_OF_REAL((2 * ref->model.zeta.q * ref->model.omega.q), TWO_ZETA_OMEGA_RES);
+ ref->model.two_omega2.q = BFP_OF_REAL((2 * ref->model.omega.q * ref->model.omega.q), TWO_OMEGA_2_RES);
+}
+
+static void update_ref_model_r(struct AttRefQuatInt *ref)
+{
+ ref->model.two_zeta_omega.r = BFP_OF_REAL((2 * ref->model.zeta.r * ref->model.omega.r), TWO_ZETA_OMEGA_RES);
+ ref->model.two_omega2.r = BFP_OF_REAL((2 * ref->model.omega.r * ref->model.omega.r), TWO_OMEGA_2_RES);
+}
+
+static void update_ref_model(struct AttRefQuatInt *ref)
+{
+ update_ref_model_p(ref);
+ update_ref_model_q(ref);
+ update_ref_model_r(ref);
+}
+
+
+void attitude_ref_quat_int_set_omega_p(struct AttRefQuatInt *ref, float omega_p)
+{
+ ref->model.omega.p = omega_p;
+ update_ref_model_p(ref);
+}
+
+void attitude_ref_quat_int_set_omega_q(struct AttRefQuatInt *ref, float omega_q)
+{
+ ref->model.omega.q = omega_q;
+ update_ref_model_q(ref);
+}
+
+void attitude_ref_quat_int_set_omega_r(struct AttRefQuatInt *ref, float omega_r)
+{
+ ref->model.omega.r = omega_r;
+ update_ref_model_r(ref);
+}
+
+void attitude_ref_quat_int_set_omega(struct AttRefQuatInt *ref, struct FloatRates *omega)
+{
+ attitude_ref_quat_int_set_omega_p(ref, omega->p);
+ attitude_ref_quat_int_set_omega_q(ref, omega->q);
+ attitude_ref_quat_int_set_omega_r(ref, omega->r);
+}
+
+void attitude_ref_quat_int_set_zeta_p(struct AttRefQuatInt *ref, float zeta_p)
+{
+ ref->model.zeta.p = zeta_p;
+ update_ref_model_p(ref);
+}
+
+void attitude_ref_quat_int_set_zeta_q(struct AttRefQuatInt *ref, float zeta_q)
+{
+ ref->model.zeta.q = zeta_q;
+ update_ref_model_q(ref);
+}
+
+void attitude_ref_quat_int_set_zeta_r(struct AttRefQuatInt *ref, float zeta_r)
+{
+ ref->model.zeta.r = zeta_r;
+ update_ref_model_r(ref);
+}
+
+void attitude_ref_quat_int_set_zeta(struct AttRefQuatInt *ref, struct FloatRates *zeta)
+{
+ attitude_ref_quat_int_set_zeta_p(ref, zeta->p);
+ attitude_ref_quat_int_set_zeta_q(ref, zeta->q);
+ attitude_ref_quat_int_set_zeta_r(ref, zeta->r);
+}
+
+
+static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi)
+{
+ ref->euler.psi = psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
+ ref->rate.r = 0;
+ ref->accel.r = 0;
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
index 52754dd537..0e1b137e36 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
@@ -32,49 +32,39 @@
#include "stabilization_attitude_ref_int.h"
-extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
-extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC
-
-void stabilization_attitude_ref_enter(void);
-
/* ref model is in float and then used to precompute ref values in int */
#include "math/pprz_algebra_float.h"
+/** Attitude reference model parameters (quat int) */
struct FloatRefModel {
struct FloatRates omega;
struct FloatRates zeta;
+ /* cached intermediate values in int */
+ struct Int32Rates two_zeta_omega;
+ struct Int32Rates two_omega2;
};
-extern struct FloatRefModel stab_att_ref_model;
+/** Attitude reference models and state/output (float) */
+struct AttRefQuatInt {
+ struct Int32Eulers euler;
+ struct Int32Quat quat;
+ struct Int32Rates rate;
+ struct Int32Rates accel;
+ struct FloatRefModel model;
+};
-extern void stabilization_attitude_ref_set_omega(struct FloatRates *omega);
-extern void stabilization_attitude_ref_set_omega_p(float omega_p);
-extern void stabilization_attitude_ref_set_omega_q(float omega_q);
-extern void stabilization_attitude_ref_set_omega_r(float omega_r);
+extern void attitude_ref_quat_int_init(struct AttRefQuatInt *ref);
+extern void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, int32_t psi);
+extern void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt);
-extern void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta);
-extern void stabilization_attitude_ref_set_zeta_p(float zeta_p);
-extern void stabilization_attitude_ref_set_zeta_q(float zeta_q);
-extern void stabilization_attitude_ref_set_zeta_r(float zeta_r);
+extern void attitude_ref_quat_int_set_omega(struct AttRefQuatInt *ref, struct FloatRates *omega);
+extern void attitude_ref_quat_int_set_omega_p(struct AttRefQuatInt *ref, float omega_p);
+extern void attitude_ref_quat_int_set_omega_q(struct AttRefQuatInt *ref, float omega_q);
+extern void attitude_ref_quat_int_set_omega_r(struct AttRefQuatInt *ref, float omega_r);
-#define stabilization_attitude_ref_quat_int_SetOmegaP(_val) { \
- stabilization_attitude_ref_set_omega_p(_val); \
- }
-#define stabilization_attitude_ref_quat_int_SetOmegaQ(_val) { \
- stabilization_attitude_ref_set_omega_q(_val); \
- }
-#define stabilization_attitude_ref_quat_int_SetOmegaR(_val) { \
- stabilization_attitude_ref_set_omega_r(_val); \
- }
-
-#define stabilization_attitude_ref_quat_int_SetZetaP(_val) { \
- stabilization_attitude_ref_set_zeta_p(_val); \
- }
-#define stabilization_attitude_ref_quat_int_SetZetaQ(_val) { \
- stabilization_attitude_ref_set_zeta_q(_val); \
- }
-#define stabilization_attitude_ref_quat_int_SetZetaR(_val) { \
- stabilization_attitude_ref_set_zeta_r(_val); \
- }
+extern void attitude_ref_quat_int_set_zeta(struct AttRefQuatInt *ref, struct FloatRates *zeta);
+extern void attitude_ref_quat_int_set_zeta_p(struct AttRefQuatInt *ref, float zeta_p);
+extern void attitude_ref_quat_int_set_zeta_q(struct AttRefQuatInt *ref, float zeta_q);
+extern void attitude_ref_quat_int_set_zeta_r(struct AttRefQuatInt *ref, float zeta_r);
#endif /* STABILIZATION_ATTITUDE_REF_QUAT_INT_H */