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

Merge branch 'master' of https://github.com/paparazzi/paparazzi

Browse Source
This commit is contained in:
TU Delft developer
2015-10-13 13:19:17 +02:00
parent b5108d9299 f261661630
commit 6807c99dec
57 changed files with 3259 additions and 736 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/rotorcraft/stabilization_indi.makefile
-1
View File
@@ -1,6 +1,5 @@
STAB_ATT_CFLAGS = -DSTABILIZATION_ATTITUDE_TYPE_INT
STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_indi.h\"
STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_indi.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c
STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
conf/settings/control/stabilization_att_float_quat.xml
+6 -6
View File
@@ -22,12 +22,12 @@
<dl_setting var="stabilization_gains[0].rates_d.z" min="0" step="1" max="500" module="stabilization/stabilization_attitude" shortname="dgaind r" param="STABILIZATION_ATTITUDE_PHI_DGAIN_D" persistent="true"/>
<dl_setting var="stabilization_gains[0].dd.z" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain r" param="STABILIZATION_ATTITUDE_PSI_DDGAIN" persistent="true"/>
<dl_setting var="stab_att_ref_model[0].omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="stab_att_ref_model[0].zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P"/>
<dl_setting var="stab_att_ref_model[0].omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="stab_att_ref_model[0].zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q"/>
<dl_setting var="stab_att_ref_model[0].omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_ref_quat_float" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_R" handler="SetOmegaR"/>
<dl_setting var="stab_att_ref_model[0].zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_ref_quat_float" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R"/>
<dl_setting var="att_ref_quat_f.model[0].omega.p" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_float" shortname="omega p" param="STABILIZATION_ATTITUDE_REF_OMEGA_P" handler="SetOmegaP"/>
<dl_setting var="att_ref_quat_f.model[0].zeta.p" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_float" shortname="zeta p" param="STABILIZATION_ATTITUDE_REF_ZETA_P"/>
<dl_setting var="att_ref_quat_f.model[0].omega.q" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_float" shortname="omega q" param="STABILIZATION_ATTITUDE_REF_OMEGA_Q" handler="SetOmegaQ"/>
<dl_setting var="att_ref_quat_f.model[0].zeta.q" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_float" shortname="zeta q" param="STABILIZATION_ATTITUDE_REF_ZETA_Q"/>
<dl_setting var="att_ref_quat_f.model[0].omega.r" min="1" step="1" max="1000" unit="rad/s" alt_unit="deg/s" module="stabilization/stabilization_attitude_quat_float" shortname="omega r" param="STABILIZATION_ATTITUDE_REF_OMEGA_R" handler="SetOmegaR"/>
<dl_setting var="att_ref_quat_f.model[0].zeta.r" min="0.5" step="0.05" max="1.2" module="stabilization/stabilization_attitude_quat_float" shortname="zeta r" param="STABILIZATION_ATTITUDE_REF_ZETA_R"/>
</dl_settings>
</dl_settings>
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/boards/bebop/video.c
+2
View File
@@ -95,6 +95,7 @@ static bool_t _write(int fd, char *data, uint8_t cnt)
/**
* Initialisation of the Aptina MT9V117 CMOS sensor
* (1/6 inch VGA, bottom camera)
*/
void mt9v117_init(void)
{
@@ -378,6 +379,7 @@ uint16_t mt9f002_read_reg16(uint16_t reg)
/**
* Initialisation of the Aptina MT9F002 CMOS sensor
* (1/2.3 inch 14Mp, front camera)
*/
void mt9f002_init(void)
{
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+31 -17
View File
@@ -35,9 +35,16 @@
#include "math/pprz_algebra_float.h"
#include "state.h"
#ifndef USE_ATT_REF
#define USE_ATT_REF 1
#endif
struct FloatAttitudeGains stabilization_gains;
struct FloatEulers stabilization_att_sum_err;
struct FloatEulers stab_att_sp_euler;
struct AttRefEulerFloat att_ref_euler_f;
float stabilization_att_fb_cmd[COMMANDS_NB];
float stabilization_att_ff_cmd[COMMANDS_NB];
@@ -76,22 +83,22 @@ 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_euler_f.euler.phi,
&att_ref_euler_f.euler.theta,
&att_ref_euler_f.euler.psi,
&att_ref_euler_f.rate.p,
&att_ref_euler_f.rate.q,
&att_ref_euler_f.rate.r,
&att_ref_euler_f.accel.p,
&att_ref_euler_f.accel.q,
&att_ref_euler_f.accel.r);
}
#endif
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
attitude_ref_euler_float_init(&att_ref_euler_f);
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
@@ -132,7 +139,7 @@ void stabilization_attitude_enter(void)
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
stabilization_attitude_ref_enter();
attitude_ref_euler_float_enter(&att_ref_euler_f, stab_att_sp_euler.psi);
FLOAT_EULERS_ZERO(stabilization_att_sum_err);
}
@@ -169,21 +176,28 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
void stabilization_attitude_run(bool_t in_flight)
{
stabilization_attitude_ref_update();
#if USE_ATT_REF
static const float dt = (1./PERIODIC_FREQUENCY);
attitude_ref_euler_float_update(&att_ref_euler_f, &stab_att_sp_euler, dt);
#else
EULERS_COPY(att_ref_euler_f.euler, stab_att_sp_euler);
FLOAT_RATES_ZERO(att_ref_euler_f.rate);
FLOAT_RATES_ZERO(att_ref_euler_f.accel);
#endif
/* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * stab_att_ref_accel.p;
stabilization_gains.dd.x * att_ref_euler_f.accel.p;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q;
stabilization_gains.dd.y * att_ref_euler_f.accel.q;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r;
stabilization_gains.dd.z * att_ref_euler_f.accel.r;
/* Compute feedback */
/* attitude error */
struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
EULERS_DIFF(att_err, att_ref_euler_f.euler, *att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -197,7 +211,7 @@ void stabilization_attitude_run(bool_t in_flight)
/* rate error */
struct FloatRates *rate_float = stateGetBodyRates_f();
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
RATES_DIFF(rate_err, att_ref_euler_f.rate, *rate_float);
/* PID */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h
+3
View File
@@ -35,4 +35,7 @@
extern struct FloatAttitudeGains stabilization_gains;
extern struct FloatEulers stabilization_att_sum_err;
extern struct FloatEulers stab_att_sp_euler;
extern struct AttRefEulerFloat att_ref_euler_f;
#endif /* STABILIZATION_ATTITUDE_EULER_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+39 -23
View File
@@ -35,6 +35,11 @@
#include "math/pprz_algebra_int.h"
#include "state.h"
/** explicitly define to zero to disable attitude reference generation */
#ifndef USE_ATTITUDE_REF
#define USE_ATTITUDE_REF 1
#endif
struct Int32AttitudeGains stabilization_gains;
/* warn if some gains are still negative */
@@ -55,12 +60,15 @@ struct Int32Eulers stabilization_att_sum_err;
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
struct Int32Eulers stab_att_sp_euler;
struct AttRefEulerInt att_ref_euler_i;
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;
att_ref_euler_i.euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
att_ref_euler_i.rate.r = 0;
att_ref_euler_i.accel.r = 0;
}
#if PERIODIC_TELEMETRY
@@ -96,22 +104,24 @@ 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_euler_i.euler.phi,
&att_ref_euler_i.euler.theta,
&att_ref_euler_i.euler.psi,
&att_ref_euler_i.rate.p,
&att_ref_euler_i.rate.q,
&att_ref_euler_i.rate.r,
&att_ref_euler_i.accel.p,
&att_ref_euler_i.accel.q,
&att_ref_euler_i.accel.r);
}
#endif
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
INT_EULERS_ZERO(stab_att_sp_euler);
attitude_ref_euler_int_init(&att_ref_euler_i);
VECT3_ASSIGN(stabilization_gains.p,
@@ -188,22 +198,28 @@ void stabilization_attitude_run(bool_t in_flight)
{
/* update reference */
stabilization_attitude_ref_update();
#if USE_ATTITUDE_REF
attitude_ref_euler_int_update(&att_ref_euler_i, &stab_att_sp_euler);
#else
INT32_EULERS_LSHIFT(att_ref_euler_i.euler, stab_att_sp_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
INT_RATES_ZERO(att_ref_euler_i.rate);
INT_RATES_ZERO(att_ref_euler_i.accel);
#endif
/* compute feedforward command */
stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(stabilization_gains.dd.x * stab_att_ref_accel.p, 5);
OFFSET_AND_ROUND(stabilization_gains.dd.x * att_ref_euler_i.accel.p, 5);
stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(stabilization_gains.dd.y * stab_att_ref_accel.q, 5);
OFFSET_AND_ROUND(stabilization_gains.dd.y * att_ref_euler_i.accel.q, 5);
stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(stabilization_gains.dd.z * stab_att_ref_accel.r, 5);
OFFSET_AND_ROUND(stabilization_gains.dd.z * att_ref_euler_i.accel.r, 5);
/* compute feedback command */
/* attitude error */
const struct Int32Eulers att_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC))
OFFSET_AND_ROUND(att_ref_euler_i.euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(att_ref_euler_i.euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(att_ref_euler_i.euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC))
};
struct Int32Eulers att_err;
struct Int32Eulers *ltp_to_body_euler = stateGetNedToBodyEulers_i();
@@ -220,9 +236,9 @@ void stabilization_attitude_run(bool_t in_flight)
/* 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_euler_i.rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(att_ref_euler_i.rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(att_ref_euler_i.rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Rates rate_err;
struct Int32Rates *body_rate = stateGetBodyRates_i();
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h
+3
View File
@@ -27,4 +27,7 @@
extern struct Int32Eulers stabilization_att_sum_err;
extern struct Int32Eulers stab_att_sp_euler;
extern struct AttRefEulerInt att_ref_euler_i;
#endif /* STABILIZATION_ATTITUDE_EULER_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+27 -17
View File
@@ -37,6 +37,11 @@
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB];
struct FloatEulers stab_att_sp_euler;
struct FloatQuat stab_att_sp_quat;
struct AttRefQuatFloat att_ref_quat_f;
struct FloatQuat stabilization_att_sum_err_quat;
struct FloatRates last_body_rate;
@@ -126,22 +131,26 @@ 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_f.euler.phi,
&att_ref_quat_f.euler.theta,
&att_ref_quat_f.euler.psi,
&att_ref_quat_f.rate.p,
&att_ref_quat_f.rate.q,
&att_ref_quat_f.rate.r,
&att_ref_quat_f.accel.p,
&att_ref_quat_f.accel.q,
&att_ref_quat_f.accel.r);
}
#endif
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
/* setpoints */
FLOAT_EULERS_ZERO(stab_att_sp_euler);
float_quat_identity(&stab_att_sp_quat);
/* reference */
attitude_ref_quat_float_init(&att_ref_quat_f);
attitude_ref_quat_float_schedule(&att_ref_quat_f, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
@@ -174,7 +183,7 @@ void stabilization_attitude_gain_schedule(uint8_t idx)
return;
}
gain_idx = idx;
stabilization_attitude_ref_schedule(idx);
attitude_ref_quat_float_schedule(&att_ref_quat_f, idx);
}
void stabilization_attitude_enter(void)
@@ -183,7 +192,7 @@ void stabilization_attitude_enter(void)
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stabilization_attitude_get_heading_f();
stabilization_attitude_ref_enter();
attitude_ref_quat_float_enter(&att_ref_quat_f, stab_att_sp_euler.psi);
float_quat_identity(&stabilization_att_sum_err_quat);
}
@@ -289,7 +298,8 @@ void stabilization_attitude_run(bool_t enable_integrator)
/*
* Update reference
*/
stabilization_attitude_ref_update();
static const float dt = (1./PERIODIC_FREQUENCY);
attitude_ref_quat_float_update(&att_ref_quat_f, &stab_att_sp_quat, dt);
/*
* Compute errors for feedback
@@ -298,14 +308,14 @@ void stabilization_attitude_run(bool_t enable_integrator)
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
float_quat_inv_comp(&att_err, att_quat, &stab_att_ref_quat);
float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
/* wrap it in the shortest direction */
float_quat_wrap_shortest(&att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates *body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
/* rate_d error */
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
@@ -324,7 +334,7 @@ void stabilization_attitude_run(bool_t enable_integrator)
float_quat_identity(&stabilization_att_sum_err_quat);
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &att_ref_quat_f.accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d,
&stabilization_att_sum_err_quat);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h
+26
View File
@@ -28,12 +28,38 @@
#ifndef STABILIZATION_ATTITUDE_QUAT_FLOAT_H
#define STABILIZATION_ATTITUDE_QUAT_FLOAT_H
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h"
#ifndef STABILIZATION_ATTITUDE_GAIN_NB
#define STABILIZATION_ATTITUDE_GAIN_NB 1
#endif
#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0
#endif
extern struct FloatAttitudeGains stabilization_gains[];
extern struct FloatEulers stab_att_sp_euler;
extern struct FloatQuat stab_att_sp_quat;
extern struct AttRefQuatFloat att_ref_quat_f;
void stabilization_attitude_gain_schedule(uint8_t idx);
/* settings handlers */
#define stabilization_attitude_quat_float_SetOmegaP(_val) { \
attitude_ref_quat_float_set_omega_p(&att_ref_quat_f, _val); \
}
#define stabilization_attitude_quat_float_SetOmegaQ(_val) { \
attitude_ref_quat_float_set_omega_q(&att_ref_quat_f, _val); \
}
#define stabilization_attitude_quat_float_SetOmegaR(_val) { \
attitude_ref_quat_float_set_omega_r(&att_ref_quat_f, _val); \
}
#endif /* STABILIZATION_ATTITUDE_QUAT_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+3 -4
View File
@@ -66,6 +66,9 @@ struct IndiVariables indi = {
{0., 0., 0.}
};
struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
// Variables for adaptation
struct FloatRates filtered_rate_estimation = {0., 0., 0.};
struct FloatRates filtered_rate_deriv_estimation = {0., 0., 0.};
@@ -129,8 +132,6 @@ static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
void stabilization_attitude_init(void)
{
stabilization_attitude_ref_init();
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_INDI", send_att_indi);
#endif
@@ -142,8 +143,6 @@ 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();
FLOAT_RATES_ZERO(indi.filtered_rate);
FLOAT_RATES_ZERO(indi.filtered_rate_deriv);
FLOAT_RATES_ZERO(indi.filtered_rate_2deriv);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h
+3
View File
@@ -64,6 +64,9 @@ extern struct ReferenceSystem reference_acceleration;
extern struct FloatRates g_est;
extern bool_t use_adaptive_indi;
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
void stabilization_indi_second_order_filter(struct FloatRates *input, struct FloatRates *filter_ddx,
struct FloatRates *filter_dx, struct FloatRates *filter_x, float omega, float zeta, float omega_r);
void lms_estimation(void);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+32 -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
@@ -95,29 +100,32 @@ static void send_att(struct transport_tx *trans, struct link_device *dev) //FI
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
{
// ref eulers in message are with REF_ANGLE_FRAC, convert
struct Int32Eulers ref_euler;
INT32_EULERS_LSHIFT(ref_euler, att_ref_quat_i.euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
&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);
&ref_euler.phi,
&ref_euler.theta,
&ref_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 +136,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 +153,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 +232,11 @@ void stabilization_attitude_run(bool_t enable_integrator)
/*
* Update reference
* Warning: dt is currently not used in the quat_int ref impl
* PERIODIC_FREQUENCY is assumed to be 512Hz
*/
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 +245,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 +275,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_euler_float.c
+41 -52
View File
@@ -25,31 +25,12 @@
* Rotorcraft attitude reference generation in euler float version.
*/
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
#include "generated/airframe.h"
struct FloatEulers stab_att_sp_euler;
struct FloatEulers stab_att_ref_euler;
struct FloatRates stab_att_ref_rate;
struct FloatRates stab_att_ref_accel;
void stabilization_attitude_ref_init(void)
{
FLOAT_EULERS_ZERO(stab_att_sp_euler);
FLOAT_EULERS_ZERO(stab_att_ref_euler);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
}
/*
* Reference
*/
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
/* shortcuts for saturation defines */
#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
@@ -58,6 +39,7 @@ void stabilization_attitude_ref_init(void)
#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
/* shortcuts for ref model parametes used in update */
#define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
#define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
@@ -66,63 +48,70 @@ void stabilization_attitude_ref_init(void)
#define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
static inline void reset_psi_ref(struct AttRefEulerFloat *ref, float psi);
#define USE_REF 1
static inline void reset_psi_ref_from_body(void)
/*
*
* Implementation.
* Should not rely on any global variables, so these functions can be used like a lib.
*
*/
void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref)
{
//sp has been set from body using stabilization_attitude_get_yaw_f, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi;
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
FLOAT_EULERS_ZERO(ref->euler);
FLOAT_RATES_ZERO(ref->rate);
FLOAT_RATES_ZERO(ref->accel);
}
void stabilization_attitude_ref_enter()
void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi)
{
reset_psi_ref_from_body();
reset_psi_ref(ref, psi);
}
void stabilization_attitude_ref_update()
void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
{
#if USE_REF
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
RATES_SMUL(delta_rate, ref->rate, dt);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(stab_att_ref_euler, delta_angle);
FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);
EULERS_ADD(ref->euler, delta_angle);
FLOAT_ANGLE_NORMALIZE(ref->euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_accel);
RATES_SMUL(delta_accel, ref->accel, dt);
RATES_ADD(ref->rate, delta_accel);
/* compute reference attitude error */
struct FloatEulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
EULERS_DIFF(ref_err, ref->euler, *sp_eulers);
/* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi;
ref->accel.p = -2.*ZETA_P * OMEGA_P * ref->rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
ref->accel.q = -2.*ZETA_Q * OMEGA_P * ref->rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
ref->accel.r = -2.*ZETA_R * OMEGA_P * ref->rate.r - OMEGA_R * OMEGA_R * ref_err.psi;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates 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 speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */
SATURATE_SPEED_TRIM_ACCEL(*ref);
}
/*
*
* Local helper functions.
*
*/
static inline void reset_psi_ref(struct AttRefEulerFloat *ref, float psi)
{
ref->euler.psi = psi;
ref->rate.r = 0;
ref->accel.r = 0;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+13 -5
View File
@@ -19,12 +19,20 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
#define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H
#ifndef STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#define STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#include "stabilization_attitude_ref_float.h"
#include "math/pprz_algebra_float.h"
void stabilization_attitude_ref_enter(void);
/** Attitude reference state/output (euler float) */
struct AttRefEulerFloat {
struct FloatEulers euler;
struct FloatRates rate;
struct FloatRates accel;
};
extern void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref);
extern void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi);
extern void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt);
#endif /* STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H */
#endif /* STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+27 -44
View File
@@ -28,20 +28,6 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
#include "generated/airframe.h"
struct Int32Eulers stab_att_sp_euler;
struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC
struct Int32Rates stab_att_ref_rate;
struct Int32Rates stab_att_ref_accel;
void stabilization_attitude_ref_init(void)
{
INT_EULERS_ZERO(stab_att_sp_euler);
INT_EULERS_ZERO(stab_att_ref_euler);
INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel);
}
#define F_UPDATE_RES 9
#define F_UPDATE (1<<F_UPDATE_RES)
@@ -84,50 +70,55 @@ void stabilization_attitude_ref_init(void)
}
/** explicitly define to zero to disable attitude reference generation */
#ifndef USE_ATTITUDE_REF
#define USE_ATTITUDE_REF 1
#endif
void stabilization_attitude_ref_update()
void attitude_ref_euler_int_init(struct AttRefEulerInt *ref)
{
INT_EULERS_ZERO(ref->euler);
INT_RATES_ZERO(ref->rate);
INT_RATES_ZERO(ref->accel);
}
#if USE_ATTITUDE_REF
/**
* Propagate reference model.
* FIXME: has hardcoded timestep (512Hz via fixed F_UPDATE_RES)
*/
void attitude_ref_euler_int_update(struct AttRefEulerInt *ref, struct Int32Eulers *sp_euler)
{
/* dumb integrate reference attitude */
const struct Int32Eulers d_angle = {
stab_att_ref_rate.p >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
stab_att_ref_rate.q >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
stab_att_ref_rate.r >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)
ref->rate.p >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
ref->rate.q >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC),
ref->rate.r >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)
};
EULERS_ADD(stab_att_ref_euler, d_angle);
ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi);
EULERS_ADD(ref->euler, d_angle);
ANGLE_REF_NORMALIZE(ref->euler.psi);
/* integrate reference rotational speeds */
const struct Int32Rates d_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, d_rate);
RATES_ADD(ref->rate, d_rate);
/* attitude setpoint with REF_ANGLE_FRAC */
struct Int32Eulers sp_ref;
INT32_EULERS_LSHIFT(sp_ref, stab_att_sp_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
INT32_EULERS_LSHIFT(sp_ref, *sp_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
/* compute reference attitude error */
struct Int32Eulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, sp_ref);
EULERS_DIFF(ref_err, ref->euler, sp_ref);
/* wrap it in the shortest direction */
ANGLE_REF_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
const struct Int32Rates accel_rate = {
((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
((int32_t)(-2.*ZETA_OMEGA_P) * (ref->rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_P_RES),
((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
((int32_t)(-2.*ZETA_OMEGA_Q) * (ref->rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_Q_RES),
((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
((int32_t)(-2.*ZETA_OMEGA_R) * (ref->rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC)))
>> (ZETA_OMEGA_R_RES)
};
@@ -137,20 +128,12 @@ void stabilization_attitude_ref_update()
((int32_t)(-OMEGA_2_R) * (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_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 speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_ATTITUDE_REF */
INT32_EULERS_LSHIFT(stab_att_ref_euler, stab_att_sp_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_ATTITUDE_REF */
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
+11
View File
@@ -29,4 +29,15 @@
#include "stabilization_attitude_ref_int.h"
/** Attitude reference models and state/output (euler int) */
struct AttRefEulerInt {
struct Int32Eulers euler; ///< with #REF_ANGLE_FRAC
struct Int32Rates rate; ///< with #REF_RATE_FRAC
struct Int32Rates accel; ///< with #REF_ACCEL_FRAC
};
extern void attitude_ref_euler_int_init(struct AttRefEulerInt *ref);
extern void attitude_ref_euler_int_enter(struct AttRefEulerInt *ref, int32_t psi);
extern void attitude_ref_euler_int_update(struct AttRefEulerInt *ref, struct Int32Eulers *sp_euler);
#endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
-49
View File
@@ -1,49 +0,0 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file stabilization_attitude_ref_float.h
* Rotorcraft attitude reference generation API.
* Common to all floating-point reference generators (euler and quaternion)
*/
#ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H
#define STABILIZATION_ATTITUDE_REF_FLOAT_H
#include "math/pprz_algebra_float.h"
extern struct FloatEulers stab_att_sp_euler;
extern struct FloatQuat stab_att_sp_quat;
extern struct FloatEulers stab_att_ref_euler;
extern struct FloatQuat stab_att_ref_quat;
extern struct FloatRates stab_att_ref_rate;
extern struct FloatRates stab_att_ref_accel;
struct FloatRefModel {
struct FloatRates omega;
struct FloatRates zeta;
};
extern struct FloatRefModel stab_att_ref_model[];
extern void stabilization_attitude_ref_init(void);
extern void stabilization_attitude_ref_update(void);
#endif /* STABILIZATION_ATTITUDE_REF_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
-8
View File
@@ -33,12 +33,4 @@
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
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 /* STABILIZATION_ATTITUDE_REF_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+118 -119
View File
@@ -32,6 +32,12 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
/// default to fast but less precise quaternion integration
#ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
#define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
#endif
/* shortcuts for saturation defines */
#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
@@ -40,17 +46,8 @@
#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
struct FloatEulers stab_att_sp_euler;
struct FloatQuat stab_att_sp_quat;
struct FloatEulers stab_att_ref_euler;
struct FloatQuat stab_att_ref_quat;
struct FloatRates stab_att_ref_rate;
struct FloatRates stab_att_ref_accel;
struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB];
static int ref_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
/* parameters used for initialization */
static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
@@ -58,147 +55,149 @@ static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
struct FloatRates two_omega_squared[STABILIZATION_ATTITUDE_GAIN_NB];
static inline void reset_psi_ref_from_body(void)
{
//sp has been set from body using stabilization_attitude_get_yaw_f, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi;
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}
static inline void update_ref_quat_from_eulers(void)
{
struct FloatRMat ref_rmat;
float_rmat_of_eulers(&ref_rmat, &stab_att_ref_euler);
float_quat_of_rmat(&stab_att_ref_quat, &ref_rmat);
float_quat_wrap_shortest(&stab_att_ref_quat);
}
void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega)
{
stab_att_ref_model[idx].omega.p = omega;
two_omega_squared[idx].p = 2 * omega * omega;
}
void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega)
{
stab_att_ref_model[idx].omega.q = omega;
two_omega_squared[idx].q = 2 * omega * omega;
}
void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega)
{
stab_att_ref_model[idx].omega.r = omega;
two_omega_squared[idx].r = 2 * omega * omega;
}
void stabilization_attitude_ref_set_omega_p(float omega)
{
stabilization_attitude_ref_idx_set_omega_p(ref_idx, omega);
}
void stabilization_attitude_ref_set_omega_q(float omega)
{
stabilization_attitude_ref_idx_set_omega_q(ref_idx, omega);
}
void stabilization_attitude_ref_set_omega_r(float omega)
{
stabilization_attitude_ref_idx_set_omega_r(ref_idx, omega);
}
void stabilization_attitude_ref_init(void)
{
FLOAT_EULERS_ZERO(stab_att_sp_euler);
float_quat_identity(&stab_att_sp_quat);
FLOAT_EULERS_ZERO(stab_att_ref_euler);
float_quat_identity(&stab_att_ref_quat);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
RATES_ASSIGN(two_omega_squared[i], 2 * omega_p[i]*omega_p[i], 2 * omega_q[i]*omega_q[i], 2 * omega_r[i]*omega_r[i]);
}
}
void stabilization_attitude_ref_schedule(uint8_t idx)
{
ref_idx = idx;
}
void stabilization_attitude_ref_enter(void)
{
reset_psi_ref_from_body();
update_ref_quat_from_eulers();
}
static inline void reset_psi_ref(struct AttRefQuatFloat *ref, float psi);
/*
* Reference
*
* Implementation.
* Should not rely on any global variables, so these functions can be used like a lib.
*
*/
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
{
FLOAT_EULERS_ZERO(ref->euler);
float_quat_identity(&ref->quat);
FLOAT_RATES_ZERO(ref->rate);
FLOAT_RATES_ZERO(ref->accel);
// default to fast but less precise quaternion integration
#ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
#define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
#endif
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
RATES_ASSIGN(ref->model[i].omega, omega_p[i], omega_q[i], omega_r[i]);
RATES_ASSIGN(ref->model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
RATES_ASSIGN(ref->model[i].two_omega2, 2 * omega_p[i]*omega_p[i], 2 * omega_q[i]*omega_q[i], 2 * omega_r[i]*omega_r[i]);
}
void stabilization_attitude_ref_update(void)
ref->cur_idx = 0;
}
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, float psi)
{
reset_psi_ref(ref, psi);
}
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
{
/* integrate reference attitude */
#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
struct FloatQuat qdot;
float_quat_derivative(&qdot, &stab_att_ref_rate, &stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(stab_att_ref_quat, qdot);
float_quat_derivative(&qdot, &ref->rate, &ref->quat);
QUAT_SMUL(qdot, qdot, dt);
QUAT_ADD(ref->quat, qdot);
#else // use finite step (involves trig)
struct FloatQuat delta_q;
float_quat_differential(&delta_q, &stab_att_ref_rate, DT_UPDATE);
float_quat_differential(&delta_q, &ref->rate, dt);
/* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
struct FloatQuat new_ref_quat;
float_quat_comp(&new_ref_quat, &stab_att_ref_quat, &delta_q);
QUAT_COPY(stab_att_ref_quat, new_ref_quat);
float_quat_comp(&new_ref_quat, &ref->quat, &delta_q);
QUAT_COPY(ref->quat, new_ref_quat);
#endif
float_quat_normalize(&stab_att_ref_quat);
float_quat_normalize(&ref->quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_rate);
RATES_SMUL(delta_rate, ref->accel, dt);
RATES_ADD(ref->rate, delta_rate);
/* compute reference angular accelerations */
struct FloatQuat err;
/* compute reference attitude error */
float_quat_inv_comp(&err, &stab_att_sp_quat, &stab_att_ref_quat);
float_quat_inv_comp(&err, sp_quat, &ref->quat);
/* wrap it in the shortest direction */
float_quat_wrap_shortest(&err);
/* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
/* since error quaternion contains the half-angles we get 2*omega^2*err */
stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p * stab_att_ref_model[ref_idx].omega.p *
stab_att_ref_rate.p
- two_omega_squared[ref_idx].p * err.qx;
stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q * stab_att_ref_model[ref_idx].omega.q *
stab_att_ref_rate.q
- two_omega_squared[ref_idx].q * err.qy;
stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r * stab_att_ref_model[ref_idx].omega.r *
stab_att_ref_rate.r
- two_omega_squared[ref_idx].r * err.qz;
ref->accel.p = -2.*ref->model[ref->cur_idx].zeta.p * ref->model[ref->cur_idx].omega.p * ref->rate.p -
ref->model[ref->cur_idx].two_omega2.p * err.qx;
ref->accel.q = -2.*ref->model[ref->cur_idx].zeta.q * ref->model[ref->cur_idx].omega.q * ref->rate.q -
ref->model[ref->cur_idx].two_omega2.q * err.qy;
ref->accel.r = -2.*ref->model[ref->cur_idx].zeta.r * ref->model[ref->cur_idx].omega.r * ref->rate.r -
ref->model[ref->cur_idx].two_omega2.r * err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates 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 */
float_eulers_of_quat(&stab_att_ref_euler, &stab_att_ref_quat);
float_eulers_of_quat(&ref->euler, &ref->quat);
}
/*
*
* Local helper functions.
*
*/
static inline void reset_psi_ref(struct AttRefQuatFloat *ref, float psi)
{
ref->euler.psi = psi;
ref->rate.r = 0;
ref->accel.r = 0;
// recalc quat from eulers
float_quat_of_eulers(&ref->quat, &ref->euler);
float_quat_wrap_shortest(&ref->quat);
}
/*
*
* Setting of the reference model parameters
*
*/
void attitude_ref_quat_float_idx_set_omega_p(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
{
ref->model[idx].omega.p = omega;
ref->model[idx].two_omega2.p = 2 * omega * omega;
}
void attitude_ref_quat_float_idx_set_omega_q(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
{
ref->model[idx].omega.q = omega;
ref->model[idx].two_omega2.q = 2 * omega * omega;
}
void attitude_ref_quat_float_idx_set_omega_r(struct AttRefQuatFloat *ref, uint8_t idx, float omega)
{
ref->model[idx].omega.r = omega;
ref->model[idx].two_omega2.r = 2 * omega * omega;
}
void attitude_ref_quat_float_set_omega_p(struct AttRefQuatFloat *ref, float omega)
{
attitude_ref_quat_float_idx_set_omega_p(ref, ref->cur_idx, omega);
}
void attitude_ref_quat_float_set_omega_q(struct AttRefQuatFloat *ref, float omega)
{
attitude_ref_quat_float_idx_set_omega_q(ref, ref->cur_idx, omega);
}
void attitude_ref_quat_float_set_omega_r(struct AttRefQuatFloat *ref, float omega)
{
attitude_ref_quat_float_idx_set_omega_r(ref, ref->cur_idx, omega);
}
/*
* schedule a different model
*/
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
{
ref->cur_idx = idx;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
+35 -24
View File
@@ -27,37 +27,48 @@
*
*/
#ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H
#define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H
#ifndef STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
#define STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
#include "stabilization_attitude_ref_float.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#ifndef STABILIZATION_ATTITUDE_GAIN_NB
#define STABILIZATION_ATTITUDE_GAIN_NB 1
#endif
#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0
#endif
/** Attitude reference model parameters (float) */
struct FloatRefModel {
struct FloatRates omega;
struct FloatRates zeta;
/// cached value of 2*omega*omega
struct FloatRates two_omega2;
};
void stabilization_attitude_ref_enter(void);
void stabilization_attitude_ref_schedule(uint8_t idx);
/** Attitude reference models and state/output (float) */
struct AttRefQuatFloat {
struct FloatEulers euler;
struct FloatQuat quat;
struct FloatRates rate;
struct FloatRates accel;
uint8_t cur_idx;
struct FloatRefModel model[STABILIZATION_ATTITUDE_GAIN_NB];
};
extern void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega);
extern void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega);
extern void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega);
extern void stabilization_attitude_ref_set_omega_p(float omega);
extern void stabilization_attitude_ref_set_omega_q(float omega);
extern void stabilization_attitude_ref_set_omega_r(float omega);
#define stabilization_attitude_ref_quat_float_SetOmegaP(_val) { \
stabilization_attitude_ref_set_omega_p(_val); \
}
#define stabilization_attitude_ref_quat_float_SetOmegaQ(_val) { \
stabilization_attitude_ref_set_omega_q(_val); \
}
#define stabilization_attitude_ref_quat_float_SetOmegaR(_val) { \
stabilization_attitude_ref_set_omega_r(_val); \
}
extern void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref);
extern void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, float psi);
extern void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt);
#endif /* STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H */
extern void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx);
extern void attitude_ref_quat_float_idx_set_omega_p(struct AttRefQuatFloat *ref, uint8_t idx, float omega);
extern void attitude_ref_quat_float_idx_set_omega_q(struct AttRefQuatFloat *ref, uint8_t idx, float omega);
extern void attitude_ref_quat_float_idx_set_omega_r(struct AttRefQuatFloat *ref, uint8_t idx, float omega);
extern void attitude_ref_quat_float_set_omega_p(struct AttRefQuatFloat *ref, float omega);
extern void attitude_ref_quat_float_set_omega_q(struct AttRefQuatFloat *ref, float omega);
extern void attitude_ref_quat_float_set_omega_r(struct AttRefQuatFloat *ref, float omega);
#endif /* STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+157 -156
View File
@@ -40,214 +40,215 @@
#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)
{
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);
}
static void update_ref_model_q(void)
{
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();
/* 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);
/* 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));
}
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);
/*
* Reference
*
* Implementation.
* Should not rely on any global variables, so these functions can be used like a lib.
*
*/
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
{
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);
}
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, int32_t psi)
{
reset_psi_ref(ref, psi);
int32_quat_of_eulers(&ref->quat, &ref->euler);
int32_quat_wrap_shortest(&ref->quat);
/* set reference rate and acceleration to zero */
memset(&ref->accel, 0, sizeof(struct Int32Rates));
memset(&ref->rate, 0, sizeof(struct Int32Rates));
}
// 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.
* CAUTION! Periodic frequency is assumed to be 512 Hz.
* 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 __attribute__((unused)))
{
/* 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));
/* compute euler representation for debugging and telemetry */
int32_eulers_of_quat(&ref->euler, &ref->quat);
}
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->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 (quat int) */
struct AttRefQuatInt {
struct Int32Eulers euler; ///< with #INT32_ANGLE_FRAC
struct Int32Quat quat;
struct Int32Rates rate; ///< with #REF_RATE_FRAC
struct Int32Rates accel; ///< with #REF_ACCEL_FRAC
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 */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h
+31 -31
View File
@@ -26,37 +26,37 @@
#ifndef STABILIZATION_ATTITUDE_REF_SATURATE_H
#define STABILIZATION_ATTITUDE_REF_SATURATE_H
#define SATURATE_SPEED_TRIM_ACCEL() { \
if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
stab_att_ref_rate.p = REF_RATE_MAX_P; \
if (stab_att_ref_accel.p > 0) \
stab_att_ref_accel.p = 0; \
} \
else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \
stab_att_ref_rate.p = -REF_RATE_MAX_P; \
if (stab_att_ref_accel.p < 0) \
stab_att_ref_accel.p = 0; \
} \
if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \
stab_att_ref_rate.q = REF_RATE_MAX_Q; \
if (stab_att_ref_accel.q > 0) \
stab_att_ref_accel.q = 0; \
} \
else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \
stab_att_ref_rate.q = -REF_RATE_MAX_Q; \
if (stab_att_ref_accel.q < 0) \
stab_att_ref_accel.q = 0; \
} \
if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \
stab_att_ref_rate.r = REF_RATE_MAX_R; \
if (stab_att_ref_accel.r > 0) \
stab_att_ref_accel.r = 0; \
} \
else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \
stab_att_ref_rate.r = -REF_RATE_MAX_R; \
if (stab_att_ref_accel.r < 0) \
stab_att_ref_accel.r = 0; \
} \
#define SATURATE_SPEED_TRIM_ACCEL(_ref) { \
if ((_ref).rate.p >= REF_RATE_MAX_P) { \
(_ref).rate.p = REF_RATE_MAX_P; \
if ((_ref).accel.p > 0) \
(_ref).accel.p = 0; \
} \
else if ((_ref).rate.p <= -REF_RATE_MAX_P) { \
(_ref).rate.p = -REF_RATE_MAX_P; \
if ((_ref).accel.p < 0) \
(_ref).accel.p = 0; \
} \
if ((_ref).rate.q >= REF_RATE_MAX_Q) { \
(_ref).rate.q = REF_RATE_MAX_Q; \
if ((_ref).accel.q > 0) \
(_ref).accel.q = 0; \
} \
else if ((_ref).rate.q <= -REF_RATE_MAX_Q) { \
(_ref).rate.q = -REF_RATE_MAX_Q; \
if ((_ref).accel.q < 0) \
(_ref).accel.q = 0; \
} \
if ((_ref).rate.r >= REF_RATE_MAX_R) { \
(_ref).rate.r = REF_RATE_MAX_R; \
if ((_ref).accel.r > 0) \
(_ref).accel.r = 0; \
} \
else if ((_ref).rate.r <= -REF_RATE_MAX_R) { \
(_ref).rate.r = -REF_RATE_MAX_R; \
if ((_ref).accel.r < 0) \
(_ref).accel.r = 0; \
} \
}
sw/airborne/test/stabilization/compare_ref_quat.py
+21 -22
View File
@@ -5,46 +5,45 @@ from __future__ import division, print_function, absolute_import
import numpy as np
import matplotlib.pyplot as plt
import ref_quat_float
import ref_quat_int
from ref_quat_float import RefQuatFloat
from ref_quat_int import RefQuatInt
steps = 512 * 2
ref_eul_res = np.zeros((steps, 3))
ref_quat_res = np.zeros((steps, 3))
ref_float_res = np.zeros((steps, 3))
ref_int_res = np.zeros((steps, 3))
ref_quat_float.init()
ref_quat_int.init()
# reset psi and update_ref_quat_from_eulers
ref_quat_float.enter()
ref_quat_int.enter()
ref_float = RefQuatFloat()
ref_int = RefQuatInt()
q_sp = np.array([0.92387956, 0.38268346, 0., 0.])
ref_quat_float.sp_quat.array = q_sp
ref_quat_int.sp_quat.array = q_sp * (1 << 15)
ref_float.setpoint = q_sp
ref_int.setpoint = q_sp
#print(ref_int.setpoint)
dt = 1/512
for i in range(0, steps):
ref_quat_float.update()
ref_eul_res[i, :] = ref_quat_float.ref_euler.array
ref_quat_int.update()
ref_quat_res[i, :] = ref_quat_int.ref_euler.array / (1 << 20)
ref_float.update(dt)
ref_float_res[i, :] = ref_float.eulers
ref_int.update(dt)
ref_int_res[i, :] = ref_int.eulers
plt.figure(1)
plt.subplot(311)
plt.title("reference in euler angles")
plt.plot(np.degrees(ref_eul_res[:, 0]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 0]), 'r')
plt.plot(np.degrees(ref_float_res[:, 0]), 'g')
plt.plot(np.degrees(ref_int_res[:, 0]), 'r')
plt.ylabel("phi [deg]")
plt.subplot(312)
plt.plot(np.degrees(ref_eul_res[:, 1]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 1]), 'r')
plt.plot(np.degrees(ref_float_res[:, 1]), 'g')
plt.plot(np.degrees(ref_int_res[:, 1]), 'r')
plt.ylabel("theta [deg]")
plt.subplot(313)
plt.plot(np.degrees(ref_eul_res[:, 2]), 'g')
plt.plot(np.degrees(ref_quat_res[:, 2]), 'r')
plt.plot(np.degrees(ref_float_res[:, 2]), 'g')
plt.plot(np.degrees(ref_int_res[:, 2]), 'r')
plt.ylabel("psi [deg]")
plt.show()
sw/airborne/test/stabilization/ref_quat_float.pxd
+24
View File
@@ -0,0 +1,24 @@
from pprz_algebra_float_c cimport FloatQuat, FloatEulers, FloatRates
cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
#struct FloatRefModel:
# FloatRates omega
# FloatRates zeta
struct AttRefQuatFloat:
FloatEulers euler
FloatQuat quat
FloatRates rate
FloatRates accel
void attitude_ref_quat_float_init(AttRefQuatFloat *ref)
void attitude_ref_quat_float_enter(AttRefQuatFloat *ref, float psi)
void attitude_ref_quat_float_update(AttRefQuatFloat *ref, FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_set_omega_p(AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_set_omega_q(AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_set_omega_r(AttRefQuatFloat *ref, float omega)
#cdef class RefQuatFloat:
# cdef AttRefQuatFloat ref
sw/airborne/test/stabilization/ref_quat_float.pyx
+23 -54
View File
@@ -1,60 +1,29 @@
cdef extern from "stdint.h":
ctypedef unsigned long int uintptr_t
cimport ref_quat_float
from pprz_algebra_float_c cimport FloatQuat
import numpy as np
cimport numpy as np
cimport pprz_algebra_float_c
from algebra_float import FloatQuat, FloatEulers, FloatRates
cdef class RefQuatFloat:
cdef ref_quat_float.AttRefQuatFloat ref
cdef FloatQuat sp_quat
def __cinit__(self):
ref_quat_float.attitude_ref_quat_float_init(&self.ref)
self.setpoint = [1.0, 0, 0, 0]
cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
void stabilization_attitude_ref_init()
void stabilization_attitude_ref_update()
void stabilization_attitude_ref_enter()
void stabilization_attitude_ref_schedule(int idx)
cpdef update(self, dt, setpoint=None):
if setpoint is not None:
self.setpoint = setpoint
ref_quat_float.attitude_ref_quat_float_update(&self.ref, &self.sp_quat, dt)
pprz_algebra_float_c.FloatEulers stab_att_sp_euler
pprz_algebra_float_c.FloatQuat stab_att_sp_quat
pprz_algebra_float_c.FloatEulers stab_att_ref_euler
pprz_algebra_float_c.FloatQuat stab_att_ref_quat
pprz_algebra_float_c.FloatRates stab_att_ref_rate
pprz_algebra_float_c.FloatRates stab_att_ref_accel
property setpoint:
def __get__(self):
return np.array([self.sp_quat.qi, self.sp_quat.qx, self.sp_quat.qy, self.sp_quat.qz])
def __set__(self, sp):
self.sp_quat.qi = sp[0]
self.sp_quat.qx = sp[1]
self.sp_quat.qy = sp[2]
self.sp_quat.qz = sp[3]
sp_euler = FloatEulers(<uintptr_t> &stab_att_sp_euler)
sp_quat = FloatQuat(<uintptr_t> &stab_att_sp_quat)
ref_euler = FloatEulers(<uintptr_t> &stab_att_ref_euler)
ref_quat = FloatQuat(<uintptr_t> &stab_att_ref_quat)
ref_rate = FloatRates(<uintptr_t> &stab_att_ref_rate)
ref_accel = FloatRates(<uintptr_t> &stab_att_ref_accel)
def init():
stabilization_attitude_ref_init()
def enter():
stabilization_attitude_ref_enter()
def update():
stabilization_attitude_ref_update()
"""
# test some different set methods
def set_sp_euler(**kwds):
global stab_att_sp_euler
cdef pprz_algebra_float_c.FloatEulers c_sp = kwds
stab_att_sp_euler = c_sp
def set_sp_euler(phi=0.0, theta=0.0, psi=0.0):
global sp_euler
sp_euler.phi = phi
sp_euler.theta = theta
sp_euler.psi = psi
def set_sp_quat(**kwds):
global stab_att_sp_quat
cdef pprz_algebra_float_c.FloatQuat c_sp = kwds
stab_att_sp_quat = c_sp
"""
property eulers:
def __get__(self):
return np.array([self.ref.euler.phi, self.ref.euler.theta, self.ref.euler.psi])
sw/airborne/test/stabilization/ref_quat_int.pxd
+23
View File
@@ -0,0 +1,23 @@
from libc.stdint cimport int32_t
from pprz_algebra_int_c cimport Int32Quat, Int32Eulers, Int32Rates
from pprz_algebra_float_c cimport FloatRates
cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
struct FloatRefModel:
FloatRates omega
FloatRates zeta
struct AttRefQuatInt:
Int32Eulers euler
Int32Quat quat
Int32Rates rate
Int32Rates accel
FloatRefModel model
void attitude_ref_quat_int_init(AttRefQuatInt *ref)
void attitude_ref_quat_int_enter(AttRefQuatInt *ref, int32_t psi)
void attitude_ref_quat_int_update(AttRefQuatInt *ref, Int32Quat *sp_quat, float dt)
void attitude_ref_quat_int_set_omega(AttRefQuatInt *ref, FloatRates *omega)
void attitude_ref_quat_int_set_zeta(AttRefQuatInt *ref, FloatRates *zeta)
sw/airborne/test/stabilization/ref_quat_int.pyx
+27 -32
View File
@@ -1,37 +1,32 @@
cdef extern from "stdint.h":
ctypedef unsigned long int uintptr_t
cimport ref_quat_int
from pprz_algebra_int_c cimport Int32Quat
import numpy as np
cdef class RefQuatInt:
cdef ref_quat_int.AttRefQuatInt ref
cdef Int32Quat sp_quat
cimport pprz_algebra_int_c
from algebra_int import Int32Quat, Int32Eulers, Int32Rates
def __cinit__(self):
ref_quat_int.attitude_ref_quat_int_init(&self.ref)
self.setpoint = [1.0, 0, 0, 0]
cpdef update(self, dt, setpoint=None):
if setpoint is not None:
self.setpoint = setpoint
ref_quat_int.attitude_ref_quat_int_update(&self.ref, &self.sp_quat, dt)
#self.c_update(&self.sp_quat, dt)
cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
void stabilization_attitude_ref_init()
void stabilization_attitude_ref_update()
void stabilization_attitude_ref_enter()
property setpoint:
def __get__(self):
quat = np.array([self.sp_quat.qi, self.sp_quat.qx, self.sp_quat.qy, self.sp_quat.qz], dtype='d')
return quat / (1 << 15)
def __set__(self, sp):
self.sp_quat.qi = sp[0] * (1 << 15)
self.sp_quat.qx = sp[1] * (1 << 15)
self.sp_quat.qy = sp[2] * (1 << 15)
self.sp_quat.qz = sp[3] * (1 << 15)
pprz_algebra_int_c.Int32Eulers stab_att_sp_euler
pprz_algebra_int_c.Int32Quat stab_att_sp_quat
pprz_algebra_int_c.Int32Eulers stab_att_ref_euler
pprz_algebra_int_c.Int32Quat stab_att_ref_quat
pprz_algebra_int_c.Int32Rates stab_att_ref_rate
pprz_algebra_int_c.Int32Rates stab_att_ref_accel
sp_euler = Int32Eulers(<uintptr_t> &stab_att_sp_euler)
sp_quat = Int32Quat(<uintptr_t> &stab_att_sp_quat)
ref_euler = Int32Eulers(<uintptr_t> &stab_att_ref_euler)
ref_quat = Int32Quat(<uintptr_t> &stab_att_ref_quat)
ref_rate = Int32Rates(<uintptr_t> &stab_att_ref_rate)
ref_accel = Int32Rates(<uintptr_t> &stab_att_ref_accel)
def init():
stabilization_attitude_ref_init()
def enter():
stabilization_attitude_ref_enter()
def update():
stabilization_attitude_ref_update()
property eulers:
def __get__(self):
euler = np.array([self.ref.euler.phi, self.ref.euler.theta, self.ref.euler.psi], dtype='d')
return euler / (1 << 20)
sw/misc/attitude_reference/Makefile
+6
View File
@@ -0,0 +1,6 @@
all:
make -C c_att_refs
clean:
make -C c_att_refs clean
sw/misc/attitude_reference/README.md
+5
View File
@@ -0,0 +1,5 @@
Tools to test and compere attitude stabilization reference generation.
In order to build the native C implementations, run `make` which will build them in the c_att_refs subdirectory.
Then simply start ./att_ref_gui.py
sw/misc/attitude_reference/att_ref_gui.py
Executable
+311
View File
@@ -0,0 +1,311 @@
#!/usr/bin/env python
#
# Copyright (C) 2014 Antoine Drouin
#
# This file is part of paparazzi.
#
# paparazzi is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2, or (at your option)
# any later version.
#
# paparazzi is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with paparazzi; see the file COPYING. If not, write to
# the Free Software Foundation, 59 Temple Place - Suite 330,
# Boston, MA 02111-1307, USA.
#
"""
This is a graphical user interface for playing with reference attitude
"""
# https://gist.github.com/zed/b966b5a04f2dfc16c98e
# https://gist.github.com/nzjrs/51686
# http://jakevdp.github.io/blog/2012/10/07/xkcd-style-plots-in-matplotlib/
# http://chimera.labs.oreilly.com/books/1230000000393/ch12.html#_problem_208 <- threads
# TODO:
# -cancel workers
#
#
#
from __future__ import print_function
from gi.repository import Gtk, GObject
from matplotlib.figure import Figure
from matplotlib.backends.backend_gtk3agg import FigureCanvasGTK3Agg as FigureCanvas
import matplotlib.font_manager as fm
import math, threading, numpy as np, scipy.signal, pdb, copy, logging
import pat.utils as pu
import pat.algebra as pa
import control as ctl
import gui
class Reference(gui.Worker):
def __init__(self, sp, ref_impl=ctl.att_ref_default, omega=6., xi=0.8, max_vel=pu.rad_of_deg(100),
max_accel=pu.rad_of_deg(500)):
gui.Worker.__init__(self)
self.impl = ref_impl()
self.sp = sp
self.reset_outputs(sp)
self.update(sp, ref_impl, omega, xi, max_vel, max_accel)
self.do_work = True
def reset_outputs(self, sp):
self.euler = np.zeros((len(sp.time), pa.e_size))
self.quat = np.zeros((len(sp.time), pa.q_size))
self.vel = np.zeros((len(sp.time), pa.r_size))
self.accel = np.zeros((len(sp.time), pa.r_size))
def update_type(self, _type):
#print('update_type', _type)
self.impl = _type()
self.do_work = True
#self.recompute()
def update_param(self, p, v):
#print('update_param', p, v)
self.impl.set_param(p, v)
self.do_work = True
#self.recompute()
def update_sp(self, sp, ref_impl=None, omega=None, xi=None, max_vel=None, max_accel=None):
self.reset_outputs(sp)
self.update(sp, ref_impl, omega, xi, max_vel, max_accel)
self.do_work = True
#self.recompute()
def update(self, sp, ref_impl=None, omega=None, xi=None, max_vel=None, max_accel=None):
self.sp = sp
if ref_impl is not None:
self.impl = ref_impl()
if omega is not None:
self.impl.set_param('omega', omega)
if xi is not None:
self.impl.set_param('xi', xi)
if max_vel is not None:
self.impl.set_param('max_vel', max_vel)
if max_accel is not None:
self.impl.set_param('max_accel', max_accel)
def recompute(self):
#print("recomputing...")
self.start((self.sp,))
def _work_init(self, sp):
#print('_work_init ', self, self.impl, sp, sp.dt)
self.euler = np.zeros((len(sp.time), pa.e_size))
self.quat = np.zeros((len(sp.time), pa.q_size))
self.vel = np.zeros((len(sp.time), pa.r_size))
self.accel = np.zeros((len(sp.time), pa.r_size))
euler0 = [0.3, 0.1, 0.2]
self.impl.set_euler(np.array(euler0))
self.quat[0], self.euler[0], self.vel[0], self.accel[0] = self.impl.quat, self.impl.euler, self.impl.vel, self.impl.accel
self.n_iter_per_step = float(len(sp.time)) / self.n_step
def _work_step(self, i, sp):
start, stop = int(i * self.n_iter_per_step), int((i + 1) * self.n_iter_per_step)
# print('_work_step of %s: i %i, start %i, stop %i' % (self.impl, i, start, stop))
for j in range(start, stop):
self.impl.update_quat(sp.quat[j], sp.dt)
self.quat[j], self.vel[j], self.accel[j] = self.impl.quat, self.impl.vel, self.impl.accel
self.euler[j] = pa.euler_of_quat(self.quat[j])
class Setpoint(object):
t_static, t_step_phi, t_step_theta, t_step_psi, t_step_random, t_nb = range(0, 6)
t_names = ["constant", "step phi", "step theta", "step psi", "step_random"]
def __init__(self, type=t_static, duration=10., step_duration=5., step_ampl=pu.rad_of_deg(10.)):
self.dt = 1. / 512
self.update(type, duration, step_duration, step_ampl)
def update(self, type, duration, step_duration, step_ampl):
self.type = type
self.duration, self.step_duration, self.step_ampl = duration, step_duration, step_ampl
self.time = np.arange(0., self.duration, self.dt)
self.euler = np.zeros((len(self.time), pa.e_size))
try:
i = [Setpoint.t_step_phi, Setpoint.t_step_theta, Setpoint.t_step_psi].index(self.type)
self.euler[:, i] = step_ampl / 2 * scipy.signal.square(math.pi / step_duration * self.time)
except Exception as e:
print(e)
pass
self.quat = np.zeros((len(self.time), pa.q_size))
for i in range(0, len(self.time)):
self.quat[i] = pa.quat_of_euler(self.euler[i])
class GUI(object):
def __init__(self, sp, refs):
self.b = Gtk.Builder()
self.b.add_from_file("ressources/att_ref_gui.xml")
w = self.b.get_object("window")
w.connect("delete-event", Gtk.main_quit)
mb = self.b.get_object("main_vbox")
self.plot = Plot(sp, refs)
mb.pack_start(self.plot, True, True, 0)
mb = self.b.get_object("main_hbox")
ref_classes = [ctl.att_ref_default, ctl.att_ref_sat_naive, ctl.att_ref_sat_nested, ctl.att_ref_sat_nested2,
ctl.AttRefFloatNative, ctl.AttRefIntNative]
self.ref_views = [gui.AttRefParamView('<b>Ref {}</b>'.format(i+1), ref_classes=ref_classes,
active_impl=r.impl) for i, r in enumerate(refs)]
for r in self.ref_views:
mb.pack_start(r, True, True, 0)
w.show_all()
class Plot(Gtk.Frame):
def __init__(self, sp, refs):
Gtk.Frame.__init__(self)
self.f = Figure()
self.canvas = FigureCanvas(self.f)
self.add(self.canvas)
self.set_size_request(1024, 600)
self.f.subplots_adjust(left=0.07, right=0.98, bottom=0.05, top=0.95,
hspace=0.2, wspace=0.2)
# self.buffer = self.canvas.get_snapshot()
def decorate(self, axis, title=None, ylab=None, legend=None):
# font_prop = fm.FontProperties(fname='Humor-Sans-1.0.ttf', size=14)
if title is not None:
axis.set_title(title) # , fontproperties=font_prop)
if ylab is not None:
axis.yaxis.set_label_text(ylab) # , fontproperties=font_prop)
if legend is not None:
axis.legend(legend) # , prop=font_prop)
axis.xaxis.grid(color='k', linestyle='-', linewidth=0.2)
axis.yaxis.grid(color='k', linestyle='-', linewidth=0.2)
def update(self, sp, refs):
title = [r'$\phi$', r'$\theta$', r'$\psi$']
legend = ['Ref1', 'Ref2', 'Setpoint']
for i in range(0, 3):
axis = self.f.add_subplot(331 + i)
axis.clear()
for ref in refs:
axis.plot(sp.time, pu.deg_of_rad(ref.euler[:, i]))
axis.plot(sp.time, pu.deg_of_rad(sp.euler[:, i]))
self.decorate(axis, title[i], *(('deg', legend) if i == 0 else (None, None)))
title = [r'$p$', r'$q$', r'$r$']
for i in range(0, 3):
axis = self.f.add_subplot(334 + i)
axis.clear()
for ref in refs:
axis.plot(sp.time, pu.deg_of_rad(ref.vel[:, i]))
self.decorate(axis, title[i], 'deg/s' if i == 0 else None)
title = [r'$\dot{p}$', r'$\dot{q}$', r'$\dot{r}$']
for i in range(0, 3):
axis = self.f.add_subplot(337 + i)
axis.clear()
for ref in refs:
axis.plot(sp.time, pu.deg_of_rad(ref.accel[:, i]))
self.decorate(axis, title[i], 'deg/s2' if i == 0 else None)
self.canvas.draw()
class Application(object):
def __init__(self):
self.sp = Setpoint()
self.refs = [Reference(self.sp), Reference(self.sp, ref_impl=ctl.AttRefFloatNative)]
for nref, r in enumerate(self.refs):
r.connect("progress", self.on_ref_update_progress, nref + 1)
r.connect("completed", self.on_ref_update_completed, nref + 1)
self.gui = GUI(self.sp, self.refs)
self.register_gui()
self.recompute_sequentially()
def on_ref_update_progress(self, ref, v, nref):
#print('progress', nref, v)
self.gui.ref_views[nref - 1].progress.set_fraction(v)
def on_ref_update_completed(self, ref, nref):
#print('on_ref_update_completed', ref, nref)
self.gui.ref_views[nref - 1].progress.set_fraction(1.0)
# recompute remaining refs (if any)
self.recompute_sequentially()
self.gui.plot.update(self.sp, self.refs)
def register_gui(self):
self.register_setpoint()
for i in range(0, 2):
self.gui.ref_views[i].connect(self._on_ref_changed, self._on_ref_param_changed, self.refs[i], self.gui.ref_views[i])
self.gui.ref_views[i].update_view(self.refs[i].impl)
def register_setpoint(self):
b = self.gui.b
c_sp_type = b.get_object("combo_sp_type")
for n in Setpoint.t_names:
c_sp_type.append_text(n)
c_sp_type.set_active(self.sp.type)
c_sp_type.connect("changed", self.on_sp_changed)
names = ["spin_sp_duration", "spin_sp_step_duration", "spin_sp_step_amplitude"]
widgets = [b.get_object(name) for name in names]
adjs = [Gtk.Adjustment(self.sp.duration, 1, 100, 1, 10, 0),
Gtk.Adjustment(self.sp.step_duration, 0.1, 10., 0.1, 1., 0),
Gtk.Adjustment(pu.deg_of_rad(self.sp.step_ampl), 0.1, 180., 1, 10., 0)]
for i, w in enumerate(widgets):
w.set_adjustment(adjs[i])
w.update()
w.connect("value-changed", self.on_sp_changed)
def recompute_sequentially(self):
"""
Somehow running two threads to update both references at the same time produces bogus data..
As a workaround we simply run them one after the other.
"""
for r in self.refs:
if r.running:
return
for r in self.refs:
if r.do_work:
r.recompute()
return
def on_sp_changed(self, widget):
b = self.gui.b
_type = b.get_object("combo_sp_type").get_active()
names = ["spin_sp_duration", "spin_sp_step_duration", "spin_sp_step_amplitude"]
_duration, _step_duration, _step_amplitude = [b.get_object(name).get_value() for name in names]
#print('_on_sp_changed', _type, _duration, _step_duration, _step_amplitude)
_step_amplitude = pu.rad_of_deg(_step_amplitude)
self.sp.update(_type, _duration, _step_duration, _step_amplitude)
# somehow running two threads to update both references at the same time produces bogus data..
# as a workaround we simply run them one after the other
for r in self.refs:
r.update_sp(self.sp)
#r.recompute()
self.recompute_sequentially()
def _on_ref_changed(self, widget, ref, view):
#print('_on_ref_changed', widget, ref, view)
ref.update_type(view.get_selected_ref_class())
view.update_ref_params(ref.impl)
self.recompute_sequentially()
def _on_ref_param_changed(self, widget, p, ref, view):
#print("_on_ref_param_changed: %s %s=%s" % (ref.impl.name, p, val))
val = view.spin_cfg[p]['d2r'](widget.get_value())
ref.update_param(p, val)
self.recompute_sequentially()
def run(self):
Gtk.main()
if __name__ == "__main__":
logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.INFO)
Application().run()
sw/misc/attitude_reference/c_att_refs/.gitignore
+4
View File
@@ -0,0 +1,4 @@
*.so
ref_quat_float.c
ref_quat_int.c
build
sw/misc/attitude_reference/c_att_refs/Makefile
+6
View File
@@ -0,0 +1,6 @@
all:
python setup.py build_ext --inplace
clean:
rm -rf build *.so *.c
sw/misc/attitude_reference/c_att_refs/README.md
+7
View File
@@ -0,0 +1,7 @@
Some Cython wrappers for stabilization reference generation testing.
Tested with Cython 0.19 - 0.21
To build execute in this directory:
python setup.py build_ext --inplace
sw/misc/attitude_reference/c_att_refs/__init__.py
View File
sw/misc/attitude_reference/c_att_refs/generated/airframe.h
+41
View File
@@ -0,0 +1,41 @@
/* fake generated airframe file */
#ifndef AIRFRAME_H
#define AIRFRAME_H
#define PERIODIC_FREQUENCY 512
#if defined STABILIZATION_ATTITUDE_TYPE_FLOAT
#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(400)}
#define STABILIZATION_ATTITUDE_REF_ZETA_P {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(400)}
#define STABILIZATION_ATTITUDE_REF_ZETA_Q {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(250)}
#define STABILIZATION_ATTITUDE_REF_ZETA_R {0.85}
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
#elif defined STABILIZATION_ATTITUDE_TYPE_INT
#define STABILIZATION_ATTITUDE_REF_OMEGA_P 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(4000.)
#define STABILIZATION_ATTITUDE_REF_OMEGA_R 4.363323125
#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
#endif
#endif // AIRFRAME_H
sw/misc/attitude_reference/c_att_refs/pprz_algebra_double_c.pxd
+29
View File
@@ -0,0 +1,29 @@
cdef extern from "math/pprz_algebra_double.h":
struct DoubleVect2
double x
double y
struct DoubleVect3
double x
double y
double z
struct DoubleEulers:
double phi
double theta
double psi
struct DoubleQuat:
double qi
double qx
double qy
double qz
struct DoubleRates:
double p
double q
double r
struct DoubleRMat:
double m[3*3]
sw/misc/attitude_reference/c_att_refs/pprz_algebra_float_c.pxd
+32
View File
@@ -0,0 +1,32 @@
cdef extern from "math/pprz_algebra_float.h":
struct FloatVect2:
float x
float y
struct FloatVect3:
float x
float y
float z
struct FloatEulers:
float phi
float theta
float psi
struct FloatQuat:
float qi
float qx
float qy
float qz
struct FloatRates:
float p
float q
float r
struct FloatRMat:
float m[3*3]
void float_eulers_of_quat(FloatEulers *e, FloatQuat *q)
void float_quat_of_eulers(FloatQuat *q, FloatEulers *e)
sw/misc/attitude_reference/c_att_refs/pprz_algebra_int_c.pxd
+37
View File
@@ -0,0 +1,37 @@
from libc.stdint cimport uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t
cdef extern from "math/pprz_algebra_int.h":
struct Int32Vect2:
int32_t x
int32_t y
struct Int32Vect3:
int32_t x
int32_t y
int32_t z
struct Int32Eulers:
int32_t phi
int32_t theta
int32_t psi
struct Int32Quat:
int32_t qi
int32_t qx
int32_t qy
int32_t qz
struct Int32Rates:
int32_t p
int32_t q
int32_t r
struct Int32RMat:
int32_t m[3*3]
void int32_quat_of_eulers(Int32Quat *q, Int32Eulers *e)
void int32_eulers_of_quat(Int32Eulers *e, Int32Quat *q)
cdef extern from "math/pprz_trig_int.h":
int16_t pprz_trig_int[]
sw/misc/attitude_reference/c_att_refs/ref_quat_float.pxd
+21
View File
@@ -0,0 +1,21 @@
from pprz_algebra_float_c cimport FloatQuat, FloatEulers, FloatRates
cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
struct FloatRefModel:
FloatRates omega
FloatRates zeta
struct AttRefQuatFloat:
FloatEulers euler
FloatQuat quat
FloatRates rate
FloatRates accel
FloatRefModel model[1]
void attitude_ref_quat_float_init(AttRefQuatFloat *ref)
void attitude_ref_quat_float_enter(AttRefQuatFloat *ref, float psi)
void attitude_ref_quat_float_update(AttRefQuatFloat *ref, FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_set_omega_p(AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_set_omega_q(AttRefQuatFloat *ref, float omega)
void attitude_ref_quat_float_set_omega_r(AttRefQuatFloat *ref, float omega)
sw/misc/attitude_reference/c_att_refs/ref_quat_float.pyx
+80
View File
@@ -0,0 +1,80 @@
cimport ref_quat_float
from pprz_algebra_float_c cimport FloatQuat, float_eulers_of_quat, float_quat_of_eulers
import numpy as np
cdef class RefQuatFloat:
cdef ref_quat_float.AttRefQuatFloat ref
cdef FloatQuat sp_quat
def __cinit__(self):
ref_quat_float.attitude_ref_quat_float_init(&self.ref)
self.setpoint = [1.0, 0, 0, 0]
cpdef update(self, dt, setpoint=None):
if setpoint is not None:
self.setpoint = setpoint
ref_quat_float.attitude_ref_quat_float_update(&self.ref, &self.sp_quat, dt)
cdef set_omega(self, omega):
ref_quat_float.attitude_ref_quat_float_set_omega_p(&self.ref, omega[0])
ref_quat_float.attitude_ref_quat_float_set_omega_q(&self.ref, omega[1])
ref_quat_float.attitude_ref_quat_float_set_omega_r(&self.ref, omega[2])
cdef set_zeta(self, zeta):
self.ref.model[0].zeta.p = zeta[0]
self.ref.model[0].zeta.q = zeta[1]
self.ref.model[0].zeta.r = zeta[2]
property setpoint:
def __get__(self):
return np.array([self.sp_quat.qi, self.sp_quat.qx, self.sp_quat.qy, self.sp_quat.qz])
def __set__(self, sp):
self.sp_quat.qi = sp[0]
self.sp_quat.qx = sp[1]
self.sp_quat.qy = sp[2]
self.sp_quat.qz = sp[3]
property euler:
def __get__(self):
return np.array([self.ref.euler.phi, self.ref.euler.theta, self.ref.euler.psi])
def __set__(self, euler):
self.ref.euler.phi = euler[0]
self.ref.euler.theta = euler[1]
self.ref.euler.psi = euler[2]
float_quat_of_eulers(&self.ref.quat, &self.ref.euler)
property quat:
def __get__(self):
return np.array([self.ref.quat.qi, self.ref.quat.qx, self.ref.quat.qy, self.ref.quat.qz], dtype='d')
def __set__(self, quat):
self.ref.quat.qi = quat[0]
self.ref.quat.qx = quat[1]
self.ref.quat.qy = quat[2]
self.ref.quat.qz = quat[3]
float_eulers_of_quat(&self.ref.euler, &self.ref.quat)
property rate:
def __get__(self):
return np.array([self.ref.rate.p, self.ref.rate.q, self.ref.rate.r])
property accel:
def __get__(self):
return np.array([self.ref.accel.p, self.ref.accel.q, self.ref.accel.r])
property zeta:
def __get__(self):
return np.array([self.ref.model[0].zeta.p, self.ref.model[0].zeta.q, self.ref.model[0].zeta.r])
def __set__(self, zeta):
if type(zeta) == float:
self.set_zeta(zeta * np.ones(3))
else:
self.set_zeta(zeta)
property omega:
def __get__(self):
return np.array([self.ref.model[0].omega.p, self.ref.model[0].omega.q, self.ref.model[0].omega.r])
def __set__(self, omega):
if type(omega) == float:
self.set_omega(omega * np.ones(3))
else:
self.set_omega(omega)
sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd
+23
View File
@@ -0,0 +1,23 @@
from libc.stdint cimport int32_t
from pprz_algebra_int_c cimport Int32Quat, Int32Eulers, Int32Rates
from pprz_algebra_float_c cimport FloatRates
cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
struct FloatRefModel:
FloatRates omega
FloatRates zeta
struct AttRefQuatInt:
Int32Eulers euler
Int32Quat quat
Int32Rates rate
Int32Rates accel
FloatRefModel model
void attitude_ref_quat_int_init(AttRefQuatInt *ref)
void attitude_ref_quat_int_enter(AttRefQuatInt *ref, int32_t psi)
void attitude_ref_quat_int_update(AttRefQuatInt *ref, Int32Quat *sp_quat, float dt)
void attitude_ref_quat_int_set_omega(AttRefQuatInt *ref, FloatRates *omega)
void attitude_ref_quat_int_set_zeta(AttRefQuatInt *ref, FloatRates *zeta)
sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx
+99
View File
@@ -0,0 +1,99 @@
cimport ref_quat_int
from pprz_algebra_int_c cimport Int32Quat, Int32Eulers, int32_quat_of_eulers, int32_eulers_of_quat
import numpy as np
REF_ACCEL_FRAC = 12
REF_RATE_FRAC = 16
INT32_ANGLE_FRAC = 12
INT32_QUAT_FRAC = 15
cdef class RefQuatInt:
cdef ref_quat_int.AttRefQuatInt ref
cdef Int32Quat sp_quat
def __cinit__(self):
ref_quat_int.attitude_ref_quat_int_init(&self.ref)
self.setpoint = [1.0, 0, 0, 0]
cpdef Int32Quat quat_bfp_of_real(self, quat):
cdef Int32Quat quat_i
quat_i.qi = quat[0] * (1 << INT32_QUAT_FRAC)
quat_i.qx = quat[1] * (1 << INT32_QUAT_FRAC)
quat_i.qy = quat[2] * (1 << INT32_QUAT_FRAC)
quat_i.qz = quat[3] * (1 << INT32_QUAT_FRAC)
return quat_i
cpdef Int32Eulers euler_bfp_of_real(self, euler):
cdef Int32Eulers euler_i
euler_i.phi = euler[0] * (1 << INT32_ANGLE_FRAC)
euler_i.theta = euler[1] * (1 << INT32_ANGLE_FRAC)
euler_i.psi = euler[2] * (1 << INT32_ANGLE_FRAC)
return euler_i
cpdef update(self, dt, setpoint=None):
if setpoint is not None:
self.setpoint = setpoint
ref_quat_int.attitude_ref_quat_int_update(&self.ref, &self.sp_quat, dt)
cdef set_omega(self, omega):
cdef FloatRates c_omega
c_omega.p = omega[0]
c_omega.q = omega[1]
c_omega.r = omega[2]
ref_quat_int.attitude_ref_quat_int_set_omega(&self.ref, &c_omega)
cdef set_zeta(self, zeta):
cdef FloatRates c_zeta
c_zeta.p = zeta[0]
c_zeta.q = zeta[1]
c_zeta.r = zeta[2]
ref_quat_int.attitude_ref_quat_int_set_zeta(&self.ref, &c_zeta)
property setpoint:
def __get__(self):
quat = np.array([self.sp_quat.qi, self.sp_quat.qx, self.sp_quat.qy, self.sp_quat.qz], dtype='d')
return quat / (1 << INT32_QUAT_FRAC)
def __set__(self, sp):
self.sp_quat = self.quat_bfp_of_real(sp)
property euler:
def __get__(self):
euler = np.array([self.ref.euler.phi, self.ref.euler.theta, self.ref.euler.psi], dtype='d')
return euler / (1 << INT32_ANGLE_FRAC)
def __set__(self, euler):
self.ref.euler = self.euler_bfp_of_real(euler)
int32_quat_of_eulers(&self.ref.quat, &self.ref.euler)
property quat:
def __get__(self):
quat = np.array([self.ref.quat.qi, self.ref.quat.qx, self.ref.quat.qy, self.ref.quat.qz], dtype='d')
return quat / (1 << INT32_QUAT_FRAC)
def __set__(self, quat):
self.ref.quat = self.quat_bfp_of_real(quat)
int32_eulers_of_quat(&self.ref.euler, &self.ref.quat)
property rate:
def __get__(self):
return np.array([self.ref.rate.p, self.ref.rate.q, self.ref.rate.r], dtype='d') / (1 << REF_RATE_FRAC)
property accel:
def __get__(self):
return np.array([self.ref.accel.p, self.ref.accel.q, self.ref.accel.r], dtype='d') / (1 << REF_ACCEL_FRAC)
property zeta:
def __get__(self):
return np.array([self.ref.model.zeta.p, self.ref.model.zeta.q, self.ref.model.zeta.r])
def __set__(self, zeta):
if type(zeta) == float:
self.set_zeta(zeta * np.ones(3))
else:
self.set_zeta(zeta)
property omega:
def __get__(self):
return np.array([self.ref.model.omega.p, self.ref.model.omega.q, self.ref.model.omega.r])
def __set__(self, omega):
if type(omega) == float:
self.set_omega(omega * np.ones(3))
else:
self.set_omega(omega)
sw/misc/attitude_reference/c_att_refs/setup.py
+33
View File
@@ -0,0 +1,33 @@
from distutils.core import setup, Extension
from Cython.Build import cythonize
from distutils.extension import Extension
import numpy
from os import path, getenv
# if PAPARAZZI_SRC not set, then assume the tree containing this
# file is a reasonable substitute
pprz_src = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
pprz_airborne = path.join(pprz_src, "sw/airborne")
common_inc_dirs = ["./", path.join(pprz_src, "sw/include"), pprz_airborne, numpy.get_include()]
includedirs = common_inc_dirs + [path.join(pprz_airborne, "firmwares/rotorcraft")]
ext_quat_float = Extension("ref_quat_float",
sources=['ref_quat_float.pyx', path.join(pprz_airborne, 'math/pprz_algebra_float.c'),
path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c")],
include_dirs=includedirs,
extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_FLOAT"])
ext_quat_int = Extension("ref_quat_int",
sources=['ref_quat_int.pyx',
path.join(pprz_airborne, 'math/pprz_trig_int.c'),
path.join(pprz_airborne, 'math/pprz_algebra_int.c'),
path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c")],
include_dirs=includedirs,
extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_INT"])
extensions = [ext_quat_float, ext_quat_int]
setup(
ext_modules=cythonize(extensions)
)
sw/misc/attitude_reference/control.py
+324
View File
@@ -0,0 +1,324 @@
#
# Copyright (C) 2014 Antoine Drouin
#
# This file is part of paparazzi.
#
# paparazzi is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2, or (at your option)
# any later version.
#
# paparazzi is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with paparazzi; see the file COPYING. If not, write to
# the Free Software Foundation, 59 Temple Place - Suite 330,
# Boston, MA 02111-1307, USA.
#
"""
This is a reference implementation of the control system to serve as specifications
for the C version
"""
import math, logging, pdb
import numpy as np
from scipy import integrate
import pat.utils as pu
import pat.algebra as pa
import pat.control as pc
from c_att_refs.ref_quat_float import RefQuatFloat
from c_att_refs.ref_quat_int import RefQuatInt
LOG = logging.getLogger('control')
# LOG.setLevel(logging.ERROR)
LOG.setLevel(logging.DEBUG)
class AttitudeReference(object):
"""Sort of abstract base class"""
def __init__(self, **kwargs):
self.omega = kwargs.get('omega', 6.)
self.xi = kwargs.get('xi', 0.8)
self.t = kwargs.get('t0', 0.)
for p in ['omega', 'xi']:
self.ensure_vect(p)
def ensure_vect(self, p):
if type(getattr(self, p)) == float:
setattr(self, p, getattr(self, p) * np.ones(3))
def set_param(self, p, v):
if hasattr(self, p):
#print("%s: setting param %s to %s" % (self.name, p, v))
if type(v) == float:
v = v * np.ones(3)
setattr(self, p, v)
def run(self, t, sp_euler=None, sp_quat=None):
dt = t - self.t
self.t = t
if sp_euler is not None:
self.update_euler(sp_euler, dt)
elif sp_quat is not None:
self.update_quat(sp_quat, dt)
def update_quat(self, setpoint, dt):
raise NotImplementedError("Should have implemented this")
def set_euler(self, euler):
raise NotImplementedError("Should have implemented this")
class att_ref_default(AttitudeReference):
"""Default second order attitude reference model, Python implementation"""
name = 'Python default'
def __init__(self, **kwargs):
super(att_ref_default, self).__init__(**kwargs)
self.euler, self.quat = np.zeros(3), pa.quat_zero()
self.vel, self.accel = np.zeros(3), np.zeros(3)
def set_quat(self, quat, vel=np.zeros(3), accel=np.zeros(3)):
self.quat = quat
self.vel = vel
self.accel = accel
self.euler = pa.euler_of_quat(self.quat)
def set_euler(self, euler, vel=np.zeros(3), accel=np.zeros(3)):
self.euler = euler
self.vel = vel
self.accel = accel
self.quat = pa.quat_of_euler(self.euler)
def update_quat(self, setpoint, dt):
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
self.vel += dt * self.accel
err_quat = pa.quat_wrap_shortest(pa.quat_inv_comp(setpoint, self.quat))
self.accel = -2. * self.xi * self.omega * self.vel \
- 2. * self.omega * self.omega * err_quat[1:]
return self.quat, self.vel, self.accel
def update_euler(self, setpoint, dt):
self.update_quat(pa.quat_of_euler(setpoint), dt)
self.euler = pa.euler_of_quat(self.quat)
class att_ref_sat_naive(att_ref_default):
"""Attitude reference model with naive saturation"""
name = 'Python Sat Naive'
def __init__(self, **kwargs):
super(att_ref_sat_naive, self).__init__(**kwargs)
self.sat_vel = kwargs.get('sat_vel', pu.rad_of_deg(100))
self.sat_accel = kwargs.get('sat_accel', pu.rad_of_deg(500))
for p in ['sat_vel', 'sat_accel']:
self.ensure_vect(p)
def update_quat(self, setpoint, dt):
super(att_ref_sat_naive, self).update_quat(setpoint, dt)
self.accel = np.clip(self.accel, -self.sat_accel, self.sat_accel)
for i in range(0, 3):
if self.vel[i] >= self.sat_vel[i]:
self.vel[i] = self.sat_vel[i]
if self.accel[i] > 0:
self.accel[i] = 0
elif self.vel[i] <= -self.sat_vel[i]:
self.vel[i] = -self.sat_vel[i]
if self.accel[i] < 0:
self.accel[i] = 0
return self.quat, self.vel, self.accel
class att_ref_sat_nested(att_ref_sat_naive):
"""Nested saturation"""
name = 'Python Sat Nested'
def __init__(self, **kwargs):
super(att_ref_sat_nested, self).__init__(**kwargs)
self._compute_auxiliary()
def update_quat(self, sp, dt):
err_quat = 2. * np.array(pa.quat_wrap_shortest(pa.quat_inv_comp(sp, self.quat)))
self.e = [err_quat[1:], self.vel, self.accel]
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
self.vel += dt * self.accel
self.accel = np.zeros(3)
self.accel = self.M[0] * np.clip(self.K[0] / self.CM[0] * self.e[0] + self.accel, -1., 1.)
self.accel = self.M[1] * np.clip(self.K[1] / self.CM[1] * self.e[1] + self.accel, -1., 1.)
# pdb.set_trace()
return self.quat, self.vel, self.accel
def set_param(self, p, v):
super(att_ref_sat_nested, self).set_param(p, v)
self._compute_auxiliary()
def _compute_auxiliary(self):
# self.K = pc.butterworth(2, self.omega[0])
omega, xi = self.omega[0], self.xi[0]
omega_d = omega * math.sqrt(1 - xi ** 2)
coefs = -np.poly([complex(-omega * xi, omega_d), complex(-omega * xi, -omega_d)])[::-1]
self.K = np.real(coefs[:-1])
LOG.debug('sat_nested.__init__ omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K)))
self.sats = np.array([self.sat_vel, self.sat_accel])
LOG.debug(' sats:\n{}'.format(self.sats))
self.M = np.array(self.sats)
self.M[0] *= self.K[1]
self.M[0] /= np.prod(self.M[1:], axis=0)
LOG.debug(' M:\n{}'.format(self.M))
self.CM = np.cumprod(self.M[::-1], axis=0)[::-1]
LOG.debug(' CM:\n{}\n'.format(self.CM))
def my_K(omega, xi=0.8):
omega_d = omega * math.sqrt(1 - xi ** 2)
p1 = complex(-omega * xi, omega_d)
p2 = complex(-omega * xi, -omega_d)
p3 = -1.5 * omega
poles = [p1, p2, p3]
coefs = -np.poly(poles)[::-1]
return np.real(coefs[:-1])
class att_ref_sat_nested2(att_ref_sat_naive):
"""Nested saturation"""
name = 'Python Sat Nested2'
def __init__(self, **kwargs):
super(att_ref_sat_nested2, self).__init__(**kwargs)
self.sat_jerk = kwargs.get('sat_jerk', pu.rad_of_deg(3000))
self.ensure_vect('sat_jerk')
self.jerk = np.zeros(3)
self._compute_auxiliary()
def update_quat(self, sp, dt):
err_quat = 2. * np.array(pa.quat_wrap_shortest(pa.quat_inv_comp(sp, self.quat)))
self.e = [err_quat[1:], self.vel, self.accel]
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
self.vel += dt * self.accel
self.accel += dt * self.jerk
self.jerk = np.zeros(3)
self.jerk = self.M[0] * np.clip(self.K[0] / self.CM[0] * self.e[0] + self.jerk, -1., 1.)
self.jerk = self.M[1] * np.clip(self.K[1] / self.CM[1] * self.e[1] + self.jerk, -1., 1.)
self.jerk = self.M[2] * np.clip(self.K[2] / self.CM[2] * self.e[2] + self.jerk, -1., 1.)
# pdb.set_trace()
return self.quat, self.vel, self.accel
def set_param(self, p, v):
super(att_ref_sat_nested2, self).set_param(p, v)
self._compute_auxiliary()
def _compute_auxiliary(self):
# self.K = pc.butterworth(3, self.omega[0])
self.K = my_K(self.omega[0], self.xi[0])
LOG.debug('sat_nested.__init__ omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K)))
self.sats = np.array([self.sat_vel, self.sat_accel, self.sat_jerk])
LOG.debug(' sats:\n{}'.format(self.sats))
self.M = np.array(self.sats)
# pdb.set_trace()
for i in range(0, 3):
self.M[0:-1, i] *= self.K[1:]
for j in range(0, 2):
self.M[1 - j, i] /= np.prod(self.M[2 - j:, i])
LOG.debug(' M:\n{}'.format(self.M))
self.CM = np.cumprod(self.M[::-1], axis=0)[::-1]
LOG.debug(' CM:\n{}\n'.format(self.CM))
class AttRefNative(AttitudeReference):
""" Base class for native C implementations """
def __init__(self, **kwargs):
super(AttRefNative, self).__init__(**kwargs)
def update_quat(self, setpoint, dt):
self.setpoint = setpoint
self.update(dt)
def set_euler(self, euler):
self.euler = euler
@property
def vel(self):
""" alias for rate """
return self.rate
@property
def xi(self):
""" alias for zeta """
return self.zeta
@xi.setter
def xi(self, value):
self.zeta = value
class AttRefFloatNative(AttRefNative, RefQuatFloat):
""" C implementation stabilization_attitude_ref_quat_float """
name = 'Native Quat Float'
def __init__(self, **kwargs):
super(AttRefFloatNative, self).__init__(**kwargs)
class AttRefIntNative(AttRefNative, RefQuatInt):
""" C implementation stabilization_attitude_ref_quat_int """
name = 'Native Quat Int'
def __init__(self, **kwargs):
super(AttRefIntNative, self).__init__(**kwargs)
class att_ref_analytic_disc(att_ref_default):
""" Scalar discrete time second order LTI"""
def __init__(self, axis=0):
super(att_ref_analytic_disc, self).__init__()
self.name = 'Scalar Analytic discrete time'
self.axis = axis
def update_euler(self, setpoint, dt):
omega, xi = self.omega[self.axis], self.xi[self.axis]
omega_d = omega * math.sqrt(1 - xi ** 2)
tan_phi = xi / math.sqrt(1 - xi ** 2)
omd_dt = omega_d * dt
s_omd_dt = math.sin(omd_dt)
c_omd_dt = math.cos(omd_dt)
a1 = tan_phi * s_omd_dt + c_omd_dt
a2 = 1. / omega_d * s_omd_dt
a3 = -omega / math.sqrt(1 - xi ** 2) * s_omd_dt
a4 = -tan_phi * s_omd_dt + c_omd_dt
emxiomdt = math.exp(-xi * omega * dt)
A = emxiomdt * np.array([[a1, a2], [a3, a4]])
B = np.array([[-emxiomdt * (tan_phi * s_omd_dt + c_omd_dt) + 1],
[omega / math.sqrt(1 - xi ** 2) * emxiomdt * s_omd_dt]])
Xi = np.array([[self.euler[self.axis]], [self.vel[self.axis]]])
Xi1 = np.dot(A, Xi) + np.dot(B, setpoint[self.axis])
self.euler[self.axis], self.vel[self.axis] = Xi1
class att_ref_analytic_cont(att_ref_default):
""" Scalar continuous time second order LTI"""
def __init__(self, axis=0):
super(att_ref_analytic_cont, self).__init__(**kwargs)
self.name = 'Scalar Analytic continuous time'
self.axis = axis
def update_euler(self, setpoint, dt):
omega, xi = self.omega[self.axis], self.xi[self.axis]
def dyn_cont(X, t, U):
Ac = np.array([[0, 1], [-omega ** 2, -2. * xi * omega]])
Bc = np.array([[0], [omega ** 2]])
return np.dot(Ac, X) + np.dot(Bc, U)
Xi = np.array([self.euler[self.axis], self.vel[self.axis]])
foo, Xi1 = integrate.odeint(dyn_cont, Xi, [0., dt], args=([setpoint[self.axis]],))
self.euler[self.axis], self.vel[self.axis] = Xi1
Xi1d = dyn_cont(Xi1, 0., [setpoint[self.axis]])
self.accel[self.axis] = Xi1d[1]
sw/misc/attitude_reference/gui.py
+132
View File
@@ -0,0 +1,132 @@
from gi.repository import Gtk, GObject
import threading
import pat.utils as pu
class Worker(GObject.GObject):
"""a worker thread that avoid blocking the UI during long tasks"""
__gsignals__ = {
"progress": (
GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [GObject.TYPE_FLOAT]),
"completed": (
GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, []),
"aborted": (
GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [])
}
def __init__(self, n_step=100):
GObject.GObject.__init__(self)
self.running = False
self.canceled = False
self.do_work = False
self.n_step = n_step
def start(self, args):
if self.running:
self.cancel()
self.t = threading.Thread(target=self._work, args=args)
self.t.daemon = True
self.t.start()
def cancel(self, wait=True):
self.canceled = True
self.do_work = False
if wait:
self.t.join()
def _emit(self, *args):
GObject.idle_add(GObject.GObject.emit, self, *args)
def _work(self, args):
self.running = True
self.canceled = False
self.do_work = False
#print("starting new work thread with args %s\nnow calling work init" % args)
self._work_init(args)
for i in range(0, self.n_step):
if self.canceled:
self.running = False
self._emit("aborted")
return
self._work_step(i, args)
self._emit("progress", float(i) / self.n_step)
self.running = False
self._emit("completed")
class AttRefParamView(Gtk.Frame):
"""a graphical user interface for editing parameters of an attitude reference"""
def __init__(self, txt=None, ref_classes=[], active_impl=None):
Gtk.Frame.__init__(self)
if txt is not None:
lab = Gtk.Label()
lab.set_markup(txt)
self.set_label_widget(lab)
self.ref_classes = ref_classes
self.b = Gtk.Builder()
self.b.add_from_file("ressources/att_ref_param_view.xml")
self.b.get_object("main_grid").reparent(self)
self.combo_type = self.b.get_object("comboboxtext_references")
for c in self.ref_classes:
self.combo_type.append_text(c.name)
self.progress = self.b.get_object("progressbar")
self.spin_cfg = {
'omega': {'range': (0.2, 20., 0.1, 1., 0.), 'r2d': lambda x: x, 'd2r': lambda x: x},
'xi': {'range': (0.1, 1.5, 0.05, 0.2, 0.), 'r2d': lambda x: x, 'd2r': lambda x: x},
'sat_vel': {'range': (1., 200., 1., 5., 0.), 'r2d': pu.deg_of_rad, 'd2r': pu.rad_of_deg},
'sat_accel': {'range': (10., 1500., 10., 20., 0.), 'r2d': pu.deg_of_rad, 'd2r': pu.rad_of_deg},
'sat_jerk': {'range': (10., 7500., 10., 20., 0.), 'r2d': pu.deg_of_rad, 'd2r': pu.rad_of_deg}
}
for n, c in self.spin_cfg.iteritems():
w = self.b.get_object("spin_{}".format(n))
adj = Gtk.Adjustment(0., *c['range'])
w.set_adjustment(adj)
if active_impl is not None:
self.update_view(active_impl)
else:
self.combo_type.set_active(0)
def connect(self, cbk_type_changed, cbk_param_changed, *args):
self.combo_type.connect("changed", cbk_type_changed, *args)
for n in self.spin_cfg.keys():
w = self.b.get_object("spin_{}".format(n))
self.spin_cfg[n]['handler'] = w.connect("value-changed", cbk_param_changed, n, *args)
def get_selected(self):
return self.combo_type.get_active()
def set_active(self, ref_class):
self.combo_type.set_active(self.ref_classes.index(ref_class))
def get_selected_ref_class(self):
return self.ref_classes[self.get_selected()]
def update_view(self, ref):
""" update view according to ref params """
self.set_active(ref.__class__)
for n, c in self.spin_cfg.iteritems():
w = self.b.get_object("spin_{}".format(n))
try:
w.set_value(c['r2d'](getattr(ref, n)[0]))
w.set_sensitive(True)
except AttributeError:
w.set_sensitive(False)
def update_ref_params(self, ref):
"""" upate ref params from view by emitting value-changed.
if param was not sensitive, update view with current param from ref
"""
for n, c in self.spin_cfg.iteritems():
w = self.b.get_object("spin_{}".format(n))
if hasattr(ref, n):
if not w.is_sensitive():
w.set_value(c['r2d'](getattr(ref, n)[0]))
w.emit('value-changed')
w.set_sensitive(True)
else:
w.set_sensitive(False)
sw/misc/attitude_reference/pat/__init__.py
View File
sw/misc/attitude_reference/pat/algebra.py
+271
View File
@@ -0,0 +1,271 @@
import math
import numpy as np
import numpy.linalg
# euler angles
e_phi = 0
e_theta = 1
e_psi = 2
e_size = 3
# rotational velocities
r_p = 0
r_q = 1
r_r = 2
r_size = 3
# quaternions
q_i = 0
q_x = 1
q_y = 2
q_z = 3
q_size = 4
# vectors
v_x = 0
v_y = 1
v_z = 2
v_size = 3
def rmat_of_euler(e):
cphi = math.cos(e[e_phi])
sphi = math.sin(e[e_phi])
ctheta = math.cos(e[e_theta])
stheta = math.sin(e[e_theta])
cpsi = math.cos(e[e_psi])
spsi = math.sin(e[e_psi])
return np.array([[ctheta * cpsi, ctheta * spsi, -stheta],
[sphi * stheta * cpsi - cphi * spsi, sphi * stheta * spsi + cphi * cpsi, sphi * ctheta],
[cphi * stheta * cpsi + sphi * spsi, cphi * stheta * spsi - sphi * cpsi, cphi * ctheta]])
def euler_of_quat(q):
dcm00 = 1.0 - 2. * (q[q_y] ** 2 + q[q_z] ** 2)
dcm01 = 2. * (q[q_x] * q[q_y] + q[q_i] * q[q_z])
dcm02 = 2. * (q[q_x] * q[q_z] - q[q_i] * q[q_y])
dcm12 = 2. * (q[q_y] * q[q_z] + q[q_i] * q[q_x])
dcm22 = 1.0 - 2. * (q[q_x] ** 2 + q[q_y] ** 2)
return [math.atan2(dcm12, dcm22),
-math.asin(dcm02),
math.atan2(dcm01, dcm00)]
def euler_of_quat_v(q):
nr, nc = q.shape
eu = np.zeros((nr, e_size))
for i in range(0, nr):
eu[i] = euler_of_quat(q[i])
return eu
#
# bad (earth relative)
#
def rmat_of_euler_1(e):
cphi = math.cos(e[e_phi])
sphi = math.sin(e[e_phi])
ctheta = math.cos(e[e_theta])
stheta = math.sin(e[e_theta])
cpsi = math.cos(e[e_psi])
spsi = math.sin(e[e_psi])
return np.array([[ctheta * cpsi, -ctheta * spsi, -stheta],
[-sphi * stheta * cpsi + cphi * spsi, sphi * stheta * spsi + cphi * cpsi, -sphi * ctheta],
[cphi * stheta * cpsi + sphi * spsi, -cphi * stheta * spsi + sphi * cpsi, cphi * ctheta]])
def rvel_of_eulerd_1(eu, eud):
sph = math.sin(eu[e_phi])
cph = math.cos(eu[e_phi])
sth = math.sin(eu[e_theta])
cth = math.cos(eu[e_theta])
a = 2 * (cph ** 2) - 1
return [eud[e_phi] - sth * eud[e_psi],
cph / a * eud[e_theta] - sph * cth / a * eud[e_psi],
-sph / a * eud[e_theta] + cph * cth / a * eud[e_psi]]
def euler_derivatives_1(eu, om):
sph = math.sin(eu[e_phi])
cph = math.cos(eu[e_phi])
tth = math.tan(eu[e_theta])
cth = math.cos(eu[e_theta])
return [om[r_p] + sph * tth * om[r_q] + cph * tth * om[r_r],
cph * om[r_q] + sph * om[r_r],
sph / cth * om[r_q] + cph / cth * om[r_r]]
#
# good (body relative)
#
def rvel_of_eulerd(eu, eud):
sph = math.sin(eu[e_phi])
cph = math.cos(eu[e_phi])
sth = math.sin(eu[e_theta])
cth = math.cos(eu[e_theta])
return [eud[e_phi] - sth * eud[e_psi],
cph * eud[e_theta] + sph * cth * eud[e_psi],
-sph * eud[e_theta] + cph * cth * eud[e_psi]]
def euler_derivatives(eu, om):
sph = math.sin(eu[e_phi])
cph = math.cos(eu[e_phi])
tth = math.tan(eu[e_theta])
cth = math.cos(eu[e_theta])
return [om[r_p] + sph * tth * om[r_q] + cph * tth * om[r_r],
cph * om[r_q] - sph * om[r_r],
sph / cth * om[r_q] + cph / cth * om[r_r]]
#
# Quaternions
#
def quat_zero():
return np.array([1., 0., 0., 0.])
def quat_of_euler(e):
s_phi2 = math.sin(e[e_phi] / 2)
c_phi2 = math.cos(e[e_phi] / 2)
s_theta2 = math.sin(e[e_theta] / 2)
c_theta2 = math.cos(e[e_theta] / 2)
s_psi2 = math.sin(e[e_psi] / 2)
c_psi2 = math.cos(e[e_psi] / 2)
return [c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2,
-c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2,
c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2,
c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2]
def quat_of_axis_angle(u, a):
sa2 = math.sin(a / 2.)
ca2 = math.cos(a / 2.)
return [ca2, sa2 * u[0], sa2 * u[1], sa2 * u[2]]
def quat_comp(a2b, b2c):
return [
a2b[q_i] * b2c[q_i] - a2b[q_x] * b2c[q_x] - a2b[q_y] * b2c[q_y] - a2b[q_z] * b2c[q_z],
a2b[q_i] * b2c[q_x] + a2b[q_x] * b2c[q_i] + a2b[q_y] * b2c[q_z] - a2b[q_z] * b2c[q_y],
a2b[q_i] * b2c[q_y] - a2b[q_x] * b2c[q_z] + a2b[q_y] * b2c[q_i] + a2b[q_z] * b2c[q_x],
a2b[q_i] * b2c[q_z] + a2b[q_x] * b2c[q_y] - a2b[q_y] * b2c[q_x] + a2b[q_z] * b2c[q_i]]
def quat_inv_comp(a2b, a2c):
return [
a2b[q_i] * a2c[q_i] + a2b[q_x] * a2c[q_x] + a2b[q_y] * a2c[q_y] + a2b[q_z] * a2c[q_z],
a2b[q_i] * a2c[q_x] - a2b[q_x] * a2c[q_i] - a2b[q_y] * a2c[q_z] + a2b[q_z] * a2c[q_y],
a2b[q_i] * a2c[q_y] + a2b[q_x] * a2c[q_z] - a2b[q_y] * a2c[q_i] - a2b[q_z] * a2c[q_x],
a2b[q_i] * a2c[q_z] - a2b[q_x] * a2c[q_y] + a2b[q_y] * a2c[q_x] - a2b[q_z] * a2c[q_i]]
def quat_derivative(q, om):
return [
0.5 * (-om[r_p] * q[q_x] - om[r_q] * q[q_y] - om[r_r] * q[q_z]),
0.5 * (om[r_p] * q[q_i] + om[r_r] * q[q_y] - om[r_q] * q[q_z]),
0.5 * (om[r_q] * q[q_i] - om[r_r] * q[q_x] + om[r_p] * q[q_z]),
0.5 * (om[r_r] * q[q_i] + om[r_q] * q[q_x] - om[r_p] * q[q_y])
]
def quat_wrap_shortest(q):
if q[q_i] < 0:
return [-q[q_i], -q[q_x], -q[q_y], -q[q_z]]
else:
return q
def quat_mormalize(q):
nq = np.linalg.norm(q)
return np.array(1. / nq * q)
def quat_vmul(q, v):
qi2 = q[q_i] * q[q_i]
qiqx = q[q_i] * q[q_x]
qiqy = q[q_i] * q[q_y]
qiqz = q[q_i] * q[q_z]
qx2 = q[q_x] * q[q_x]
qxqy = q[q_x] * q[q_y]
qxqz = q[q_x] * q[q_z]
qy2 = q[q_y] * q[q_y]
qyqz = q[q_y] * q[q_z]
qz2 = q[q_z] * q[q_z]
m00 = qi2 + qx2 - qy2 - qz2
m01 = 2 * (qxqy + qiqz)
m02 = 2 * (qxqz - qiqy)
m10 = 2 * (qxqy - qiqz)
m11 = qi2 - qx2 + qy2 - qz2
m12 = 2 * (qyqz + qiqx)
m20 = 2 * (qxqz + qiqy)
m21 = 2 * (qyqz - qiqx)
m22 = qi2 - qx2 - qy2 + qz2
return [
m00 * v[v_x] + m01 * v[v_y] + m02 * v[v_z],
m10 * v[v_x] + m11 * v[v_y] + m12 * v[v_z],
m20 * v[v_x] + m21 * v[v_y] + m22 * v[v_z]]
def quat_inv_vmul(q, v):
qi2 = q[q_i] * q[q_i]
qiqx = q[q_i] * q[q_x]
qiqy = q[q_i] * q[q_y]
qiqz = q[q_i] * q[q_z]
qx2 = q[q_x] * q[q_x]
qxqy = q[q_x] * q[q_y]
qxqz = q[q_x] * q[q_z]
qy2 = q[q_y] * q[q_y]
qyqz = q[q_y] * q[q_z]
qz2 = q[q_z] * q[q_z]
m00 = qi2 + qx2 - qy2 - qz2
m01 = 2 * (qxqy + qiqz)
m02 = 2 * (qxqz - qiqy)
m10 = 2 * (qxqy - qiqz)
m11 = qi2 - qx2 + qy2 - qz2
m12 = 2 * (qyqz + qiqx)
m20 = 2 * (qxqz + qiqy)
m21 = 2 * (qyqz - qiqx)
m22 = qi2 - qx2 - qy2 + qz2
return [
m00 * v[v_x] + m10 * v[v_y] + m20 * v[v_z],
m01 * v[v_x] + m11 * v[v_y] + m21 * v[v_z],
m02 * v[v_x] + m12 * v[v_y] + m22 * v[v_z]]
def quat_integrate(q_im1, om_im1, dt):
no = np.linalg.norm(om_im1)
if no == 0.:
codt2 = 1.
sodt2o = 1.
else:
codt2 = math.cos(dt / 2. * no)
sodt2o = math.sin(dt / 2. * no) / no
p = -om_im1[0]
q = -om_im1[1]
r = -om_im1[2]
# WTF wrong formula. inverted signs instead
Phi = np.array([[codt2, p * sodt2o, q * sodt2o, r * sodt2o],
[-p * sodt2o, codt2, -r * sodt2o, q * sodt2o],
[-q * sodt2o, r * sodt2o, codt2, -p * sodt2o],
[-r * sodt2o, -q * sodt2o, p * sodt2o, codt2]])
q_i = np.dot(Phi, q_im1)
return q_i
def cross_product(v1, v2):
return np.array([v1[v_y] * v2[v_z] - v1[v_z] * v2[v_y],
v1[v_z] * v2[v_x] - v1[v_x] * v2[v_z],
v1[v_x] * v2[v_y] - v1[v_y] * v2[v_x]])
def dot_product(v1, v2):
return np.sum(v1 * v2)

Some files were not shown because too many files have changed in this diff Show More