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

[rotorcraft] attitude ref: refactor quat_int

Browse Source
This commit is contained in:
Felix Ruess
2015-10-04 00:06:46 +02:00
parent 15f2eacc07
commit e73b824185
6 changed files with 236 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/control/stabilization_att_int_quat.xml
+6 -6
View File
@@ -17,12 +17,12 @@
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" module="stabilization/stabilization_attitude_common_int" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN" type="int32" persistent="true"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude_common_int" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN" type="int32" persistent="true"/>
<dl_setting var="stab_att_ref_model.omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="stab_att_ref_model.zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P" handler="SetZetaP"/>
<dl_setting var="stab_att_ref_model.omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="stab_att_ref_model.zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q" handler="SetZetaQ"/>
<dl_setting var="stab_att_ref_model.omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_int" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_R" handler="SetOmegaR"/>
<dl_setting var="stab_att_ref_model.zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_int" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R" handler="SetZetaR"/>
<dl_setting var="att_ref_quat_i.model.omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_int" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="att_ref_quat_i.model.zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_int" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P" handler="SetZetaP"/>
<dl_setting var="att_ref_quat_i.model.omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_int" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="att_ref_quat_i.model.zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_int" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q" handler="SetZetaQ"/>
<dl_setting var="att_ref_quat_i.model.omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_int" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_R" handler="SetOmegaR"/>
<dl_setting var="att_ref_quat_i.model.zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_int" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R" handler="SetZetaR"/>
</dl_settings>
</dl_settings>
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+28 -21
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h
+26
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+2 -3
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+151 -147
View File
@@ -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;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+23 -33
View File
@@ -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 */