diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_indi.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_indi.makefile index 6e561aa342..64c6ce90da 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_indi.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_indi.makefile @@ -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 diff --git a/conf/settings/control/stabilization_att_float_quat.xml b/conf/settings/control/stabilization_att_float_quat.xml index 3bdefb1bb2..0f103da585 100644 --- a/conf/settings/control/stabilization_att_float_quat.xml +++ b/conf/settings/control/stabilization_att_float_quat.xml @@ -22,12 +22,12 @@ - - - - - - + + + + + + diff --git a/conf/settings/control/stabilization_att_int_quat.xml b/conf/settings/control/stabilization_att_int_quat.xml index b76196174b..fd84e02734 100644 --- a/conf/settings/control/stabilization_att_int_quat.xml +++ b/conf/settings/control/stabilization_att_int_quat.xml @@ -17,12 +17,12 @@ - - - - - - + + + + + + diff --git a/sw/airborne/boards/bebop/video.c b/sw/airborne/boards/bebop/video.c index 02ce1de85e..cef73a2b46 100644 --- a/sw/airborne/boards/bebop/video.c +++ b/sw/airborne/boards/bebop/video.c @@ -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) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 02ec04bd55..6da16c6b05 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h index 08e4dd235c..ca9a044d71 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 0effe51aef..65a0c4e848 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -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(); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h index eba7a64b82..996c5c1a39 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 0da354e16a..cf7e1817ec 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h index 8655fc23ac..7edc3d11ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c index 79a198b587..ba6fd36851 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h index 11492a138d..8b68e65827 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.h @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 7cc023ccce..c57804a256 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -60,6 +60,11 @@ struct Int32Quat stabilization_att_sum_err_quat; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; +struct Int32Quat stab_att_sp_quat; +struct Int32Eulers stab_att_sp_euler; + +struct AttRefQuatInt att_ref_quat_i; + #define IERROR_SCALE 128 #define GAIN_PRESCALER_FF 48 #define GAIN_PRESCALER_P 12 @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h index 84c954ac86..b9a05837cd 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.h @@ -27,4 +27,30 @@ #include "math/pprz_algebra_int.h" +extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC +extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC + +extern struct AttRefQuatInt att_ref_quat_i; + +/* settings handlers for ref model params */ +#define stabilization_attitude_quat_int_SetOmegaP(_val) { \ + attitude_ref_quat_int_set_omega_p(&att_ref_quat_i, _val); \ + } +#define stabilization_attitude_quat_int_SetOmegaQ(_val) { \ + attitude_ref_quat_int_set_omega_q(&att_ref_quat_i, _val); \ + } +#define stabilization_attitude_quat_int_SetOmegaR(_val) { \ + attitude_ref_quat_int_set_omega_r(&att_ref_quat_i, _val); \ + } + +#define stabilization_attitude_quat_int_SetZetaP(_val) { \ + attitude_ref_quat_int_set_zeta_p(&att_ref_quat_i, _val); \ + } +#define stabilization_attitude_quat_int_SetZetaQ(_val) { \ + attitude_ref_quat_int_set_zeta_q(&att_ref_quat_i, _val); \ + } +#define stabilization_attitude_quat_int_SetZetaR(_val) { \ + attitude_ref_quat_int_set_zeta_r(&att_ref_quat_i, _val); \ + } + #endif /* STABILIZATION_ATTITUDE_QUAT_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index de93c7be81..b9bc4524f7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -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; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index ba55615872..f09ece8669 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index fa7f6a4b20..336b882210 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -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<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 */ - } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index 08579a412a..accf8e89b6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h deleted file mode 100644 index fb62bcc457..0000000000 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2008-2009 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. - */ - -/** @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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index b004b357cf..7c20a5122a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -33,12 +33,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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index ea3aab080a..14f6206b9a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -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; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index dfb8ee2031..38cc9081da 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index abf5dcf762..57da453133 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -40,214 +40,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; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index 52754dd537..20612318ad 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -32,49 +32,39 @@ #include "stabilization_attitude_ref_int.h" -extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC -extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC - -void stabilization_attitude_ref_enter(void); - /* ref model is in float and then used to precompute ref values in int */ #include "math/pprz_algebra_float.h" +/** Attitude reference model parameters (quat int) */ struct FloatRefModel { struct FloatRates omega; struct FloatRates zeta; + /* cached intermediate values in int */ + struct Int32Rates two_zeta_omega; + struct Int32Rates two_omega2; }; -extern struct FloatRefModel stab_att_ref_model; +/** Attitude reference models and state/output (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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h index 30fac212a3..3452022552 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h @@ -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; \ + } \ } diff --git a/sw/airborne/test/stabilization/compare_ref_quat.py b/sw/airborne/test/stabilization/compare_ref_quat.py index 76fc8d3a15..0ccdfd3bea 100755 --- a/sw/airborne/test/stabilization/compare_ref_quat.py +++ b/sw/airborne/test/stabilization/compare_ref_quat.py @@ -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() diff --git a/sw/airborne/test/stabilization/ref_quat_float.pxd b/sw/airborne/test/stabilization/ref_quat_float.pxd new file mode 100644 index 0000000000..58fea98e11 --- /dev/null +++ b/sw/airborne/test/stabilization/ref_quat_float.pxd @@ -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 diff --git a/sw/airborne/test/stabilization/ref_quat_float.pyx b/sw/airborne/test/stabilization/ref_quat_float.pyx index 1509ee3e87..899efa15e8 100644 --- a/sw/airborne/test/stabilization/ref_quat_float.pyx +++ b/sw/airborne/test/stabilization/ref_quat_float.pyx @@ -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( &stab_att_sp_euler) -sp_quat = FloatQuat( &stab_att_sp_quat) -ref_euler = FloatEulers( &stab_att_ref_euler) -ref_quat = FloatQuat( &stab_att_ref_quat) -ref_rate = FloatRates( &stab_att_ref_rate) -ref_accel = FloatRates( &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]) diff --git a/sw/airborne/test/stabilization/ref_quat_int.pxd b/sw/airborne/test/stabilization/ref_quat_int.pxd new file mode 100644 index 0000000000..30aaa5a254 --- /dev/null +++ b/sw/airborne/test/stabilization/ref_quat_int.pxd @@ -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) diff --git a/sw/airborne/test/stabilization/ref_quat_int.pyx b/sw/airborne/test/stabilization/ref_quat_int.pyx index 0e4d265495..e81344ffe9 100644 --- a/sw/airborne/test/stabilization/ref_quat_int.pyx +++ b/sw/airborne/test/stabilization/ref_quat_int.pyx @@ -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( &stab_att_sp_euler) -sp_quat = Int32Quat( &stab_att_sp_quat) -ref_euler = Int32Eulers( &stab_att_ref_euler) -ref_quat = Int32Quat( &stab_att_ref_quat) -ref_rate = Int32Rates( &stab_att_ref_rate) -ref_accel = Int32Rates( &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) diff --git a/sw/misc/attitude_reference/Makefile b/sw/misc/attitude_reference/Makefile new file mode 100644 index 0000000000..c6c52f502f --- /dev/null +++ b/sw/misc/attitude_reference/Makefile @@ -0,0 +1,6 @@ + +all: + make -C c_att_refs + +clean: + make -C c_att_refs clean diff --git a/sw/misc/attitude_reference/README.md b/sw/misc/attitude_reference/README.md new file mode 100644 index 0000000000..64ffb7c189 --- /dev/null +++ b/sw/misc/attitude_reference/README.md @@ -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 diff --git a/sw/misc/attitude_reference/att_ref_gui.py b/sw/misc/attitude_reference/att_ref_gui.py new file mode 100755 index 0000000000..4a1f4c2611 --- /dev/null +++ b/sw/misc/attitude_reference/att_ref_gui.py @@ -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('Ref {}'.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() diff --git a/sw/misc/attitude_reference/c_att_refs/.gitignore b/sw/misc/attitude_reference/c_att_refs/.gitignore new file mode 100644 index 0000000000..c77768018e --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/.gitignore @@ -0,0 +1,4 @@ +*.so +ref_quat_float.c +ref_quat_int.c +build diff --git a/sw/misc/attitude_reference/c_att_refs/Makefile b/sw/misc/attitude_reference/c_att_refs/Makefile new file mode 100644 index 0000000000..f05ebefd77 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/Makefile @@ -0,0 +1,6 @@ + +all: + python setup.py build_ext --inplace + +clean: + rm -rf build *.so *.c diff --git a/sw/misc/attitude_reference/c_att_refs/README.md b/sw/misc/attitude_reference/c_att_refs/README.md new file mode 100644 index 0000000000..c9551f814c --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/README.md @@ -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 diff --git a/sw/misc/attitude_reference/c_att_refs/__init__.py b/sw/misc/attitude_reference/c_att_refs/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/misc/attitude_reference/c_att_refs/generated/airframe.h b/sw/misc/attitude_reference/c_att_refs/generated/airframe.h new file mode 100644 index 0000000000..89e9d1b06b --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/generated/airframe.h @@ -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 diff --git a/sw/misc/attitude_reference/c_att_refs/pprz_algebra_double_c.pxd b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_double_c.pxd new file mode 100644 index 0000000000..e2204d25b8 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_double_c.pxd @@ -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] diff --git a/sw/misc/attitude_reference/c_att_refs/pprz_algebra_float_c.pxd b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_float_c.pxd new file mode 100644 index 0000000000..287fe8ac39 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_float_c.pxd @@ -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) diff --git a/sw/misc/attitude_reference/c_att_refs/pprz_algebra_int_c.pxd b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_int_c.pxd new file mode 100644 index 0000000000..e4ca2f2d1e --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/pprz_algebra_int_c.pxd @@ -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[] diff --git a/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pxd b/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pxd new file mode 100644 index 0000000000..33607ecb40 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pxd @@ -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) diff --git a/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pyx b/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pyx new file mode 100644 index 0000000000..aea8988ae0 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_float.pyx @@ -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) diff --git a/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd new file mode 100644 index 0000000000..30aaa5a254 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd @@ -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) diff --git a/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx new file mode 100644 index 0000000000..100e80bf7e --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx @@ -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) diff --git a/sw/misc/attitude_reference/c_att_refs/setup.py b/sw/misc/attitude_reference/c_att_refs/setup.py new file mode 100644 index 0000000000..d49a34c2a5 --- /dev/null +++ b/sw/misc/attitude_reference/c_att_refs/setup.py @@ -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) +) diff --git a/sw/misc/attitude_reference/control.py b/sw/misc/attitude_reference/control.py new file mode 100644 index 0000000000..1acab6e5a5 --- /dev/null +++ b/sw/misc/attitude_reference/control.py @@ -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] diff --git a/sw/misc/attitude_reference/gui.py b/sw/misc/attitude_reference/gui.py new file mode 100644 index 0000000000..8331a00049 --- /dev/null +++ b/sw/misc/attitude_reference/gui.py @@ -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) diff --git a/sw/misc/attitude_reference/pat/__init__.py b/sw/misc/attitude_reference/pat/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/misc/attitude_reference/pat/algebra.py b/sw/misc/attitude_reference/pat/algebra.py new file mode 100644 index 0000000000..d3fa6efd42 --- /dev/null +++ b/sw/misc/attitude_reference/pat/algebra.py @@ -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) diff --git a/sw/misc/attitude_reference/pat/control.py b/sw/misc/attitude_reference/pat/control.py new file mode 100644 index 0000000000..9ff52bea97 --- /dev/null +++ b/sw/misc/attitude_reference/pat/control.py @@ -0,0 +1,112 @@ +from __future__ import division + +import math +import numpy as np + + +class LinRef: + """ Linear Reference Model (with first order integration)""" + + def __init__(self, K): + """K: coefficients of the caracteristic polynomial, in ascending powers order, + highesr order ommited (normalized to -1)""" + self.K = K + self.order = len(K) + self.X = np.zeros(self.order + 1) + + def run(self, dt, sp): + self.X[:self.order] += self.X[1:self.order + 1] * dt + e = np.array(self.X[:self.order]) + e[0] -= sp + self.X[self.order] = np.sum(e * self.K) + return self.X + + +class SatRef: + """ Nested Saturations Reference Model (with first order integration)""" + + def __init__(self, K, sats): + """ + K: coefficients of the caracteristic polynomial, in ascending power order, + highest power ommited (normalized to -1) + sats: saturations for each order in ascending order + """ + self.K = K + self.M = np.array(sats) + self.M[0:-1] *= K[1:] + for i in range(0, len(self.M) - 1): + self.M[len(self.M) - 2 - i] /= np.prod(self.M[len(self.M) - 1 - i:]) + self.CM = np.cumprod(self.M[::-1])[::-1] + print 'M', self.M, 'CM', self.CM + self.order = len(K) + self.X = np.zeros(self.order + 1) + + def run(self, dt, sp): + self.X[:self.order] += self.X[1:self.order + 1] * dt + e = np.array(self.X[:self.order]) + e[0] -= sp + self.X[self.order] = 0 + for i in range(0, self.order): + self.X[self.order] = self.M[i] * np.clip(self.K[i] / self.CM[i] * e[i] + self.X[self.order], -1., 1.) + return self.X + + +class SatRefNaiveSecOrder: + """ Second Order (brutally) Saturated Reference Model""" + + def __init__(self, K, sats): + """ + K: coefficients of the caracteristic polynomial, in ascending power order, + highest power ommited (normalized to -1) + sats: saturations for each order in ascending order + """ + self.K = K + self.sats = sats + self.order = len(K) + self.X = np.zeros(self.order + 1) + + def run(self, dt, sp): + self.X[:self.order] += self.X[1:self.order + 1] * dt + e = np.array(self.X[:self.order]) + e[0] -= sp + self.X[self.order] = np.sum(e * self.K) + self.X[self.order] = np.clip(self.X[self.order], -self.sats[1], self.sats[1]) # saturate accel + if self.X[self.order - 1] >= self.sats[0]: # saturate vel, trim accel + self.X[self.order - 1] = self.sats[0] + if self.X[self.order] > 0: + self.X[self.order] = 0 + elif self.X[self.order - 1] <= -self.sats[0]: + self.X[self.order - 1] = -self.sats[0] + if self.X[self.order] < 0: + self.X[self.order] = 0 + return self.X + + +def butterworth(n, omega=1): + """ + returns the coefficients of a butterworth polynomial of order n in ascending powers order + see: http://en.wikipedia.org/wiki/Butterworth_filter + """ + poles = [omega * np.exp(complex(0, 2 * k + n - 1) * math.pi / 2 / n) for k in range(1, n + 1)] + coefs = -np.poly(poles)[::-1] + return np.real(coefs[:-1]) + + +def bessel(n, omega=1): + """ + returns the coefficients of a bessel polynomial of order n in ascending powers order + see http://en.wikipedia.org/wiki/Bessel_filter + """ + coefs = [-math.factorial(2 * n - k) / (math.pow(2, n - k) * math.factorial(k) * math.factorial(n - k)) for k in + range(0, n)] + return coefs[::-1] + + +def poles(K): + """ returns the roots of a polynomial whose coefficients are provided in ascending order, + highest power coefficients normalized to -1 + """ + p = np.zeros(len(K) + 1) + p[0] = -1 + p[1:] = K[::-1] + return np.roots(p) diff --git a/sw/misc/attitude_reference/pat/utils.py b/sw/misc/attitude_reference/pat/utils.py new file mode 100644 index 0000000000..814555e591 --- /dev/null +++ b/sw/misc/attitude_reference/pat/utils.py @@ -0,0 +1,259 @@ +# +# Copyright 2013-2014 Antoine Drouin (poinix@gmail.com) +# +# This file is part of PAT. +# +# PAT 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 3 of the License, or +# (at your option) any later version. +# +# PAT 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 PAT. If not, see . +# + +""" +Utility functions +""" +import math +import numpy as np +import numpy.linalg as linalg +import pdb + +""" +Unit convertions +""" + + +def rad_of_deg(d): + return d / 180. * math.pi + + +def sqrad_of_sqdeg(d): + return d / (180. * math.pi) ** 2 + + +def deg_of_rad(r): + return r * 180. / math.pi + + +def sqdeg_of_sqrad(r): + return r * (180. / math.pi) ** 2 + + +def rps_of_rpm(r): + return r * 2. * math.pi / 60. + + +def rpm_of_rps(r): + return r / 2. / math.pi * 60. + + +# http://en.wikipedia.org/wiki/Nautical_mile +def m_of_NM(nm): + return nm * 1852. + + +def NM_of_m(m): + return m / 1852. + + +# http://en.wikipedia.org/wiki/Knot_(speed) +def mps_of_kt(kt): + return kt * 0.514444 + + +def kt_of_mps(mps): + return mps / 0.514444 + + +# http://en.wikipedia.org/wiki/Foot_(unit) +def m_of_ft(ft): + return ft * 0.3048 + + +def ft_of_m(m): + return m / 0.3048 + + +# feet per minute to/from meters per second +def ftpm_of_mps(mps): + return mps * 60. * 3.28084 + + +def mps_of_ftpm(ftpm): + return ftpm / 60. / 3.28084 + + +""" +Cliping +""" + + +def norm_angle_0_2pi(a): + while a > 2. * math.pi: + a -= 2. * math.pi + while a <= 0: + a += 2. * math.pi + return a + + +def norm_angle_mpi_pi(a): + while a > math.pi: + a -= 2. * math.pi + while a <= -math.pi: + a += 2. * math.pi + return a + + +# +def saturate(_v, _min, _max): + if _v < _min: + return _min + if _v > _max: + return _max + return _v + + +""" +Plotting +""" +import matplotlib +import matplotlib.pyplot as plt +import matplotlib.gridspec as gridspec + +my_title_spec = {'color': 'k', 'fontsize': 20} + + +def save_if(filename): + if filename: matplotlib.pyplot.savefig(filename, dpi=80) + + +def prepare_fig(fig=None, window_title=None, figsize=(20.48, 10.24), margins=None): + if fig is None: + fig = plt.figure(figsize=figsize) + # else: + # plt.figure(fig.number) + if margins: + left, bottom, right, top, wspace, hspace = margins + fig.subplots_adjust(left=left, right=right, bottom=bottom, top=top, + hspace=hspace, wspace=wspace) + if window_title: + fig.canvas.set_window_title(window_title) + return fig + + +def decorate(ax, title=None, xlab=None, ylab=None, legend=None, xlim=None, ylim=None): + ax.xaxis.grid(color='k', linestyle='-', linewidth=0.2) + ax.yaxis.grid(color='k', linestyle='-', linewidth=0.2) + if xlab: + ax.xaxis.set_label_text(xlab) + if ylab: + ax.yaxis.set_label_text(ylab) + if title: + ax.set_title(title, my_title_spec) + if legend is not None: + ax.legend(legend, loc='best') + if xlim is not None: + ax.set_xlim(xlim[0], xlim[1]) + if ylim is not None: + ax.set_ylim(ylim[0], ylim[1]) + + +def ensure_ylim(ax, yspan): + ymin, ymax = ax.get_ylim() + if ymax - ymin < yspan: + ym = (ymin + ymax) / 2 + ax.set_ylim(ym - yspan / 2, ym + yspan / 2) + + +def write_text(nrows, ncols, plot_number, text, colspan=1, loc=[[0.5, 9.7]], filename=None): + # ax = plt.subplot(nrows, ncols, plot_number) + gs = gridspec.GridSpec(nrows, ncols) + row, col = divmod(plot_number - 1, ncols) + ax = plt.subplot(gs[row, col:col + colspan]) + plt.axis([0, 10, 0, 10]) + ax.get_xaxis().set_visible(False) + ax.get_yaxis().set_visible(False) + for i in range(0, len(text)): + plt.text(loc[i][0], loc[i][1], text[i], ha='left', va='top') + save_if(filename) + + +def plot_in_grid(time, plots, ncol, figure=None, window_title="None", legend=None, filename=None, + margins=(0.04, 0.08, 0.93, 0.96, 0.20, 0.34)): + nrow = math.ceil(len(plots) / float(ncol)) + figsize = (10.24 * ncol, 2.56 * nrow) + figure = prepare_fig(figure, window_title, figsize=figsize, margins=margins) + # pdb.set_trace() + for i, (title, ylab, data) in enumerate(plots): + ax = figure.add_subplot(nrow, ncol, i + 1) + ax.plot(time, data) + decorate(ax, title=title, ylab=ylab) + if legend is not None: + ax.legend(legend, loc='best') + save_if(filename) + return figure + + +""" +Misc +""" + + +def num_jacobian(X, U, P, dyn): + s_size = len(X) + i_size = len(U) + epsilonX = (0.1 * np.ones(s_size)).tolist() + dX = np.diag(epsilonX) + A = np.zeros((s_size, s_size)) + for i in range(0, s_size): + dx = dX[i, :] + delta_f = dyn(X + dx / 2, 0, U, P) - dyn(X - dx / 2, 0, U, P) + delta_f = delta_f / dx[i] + # print delta_f + A[:, i] = delta_f + + epsilonU = (0.1 * np.ones(i_size)).tolist() + dU = np.diag(epsilonU) + B = np.zeros((s_size, i_size)) + for i in range(0, i_size): + du = dU[i, :] + delta_f = dyn(X, 0, U + du / 2, P) - dyn(X, 0, U - du / 2, P) + delta_f = delta_f / du[i] + B[:, i] = delta_f + + return A, B + + +def saturate(V, Sats): + Vsat = np.array(V) + for i in range(0, len(V)): + if Vsat[i] < Sats[i, 0]: + Vsat[i] = Sats[i, 0] + elif Vsat[i] > Sats[i, 1]: + Vsat[i] = Sats[i, 1] + return Vsat + + +def print_lti_dynamics(A, B, txt=None, print_original_form=False, print_modal_form=False): + if txt: + print txt + if print_original_form: + print "A\n", A + print "B\n", B + w, M = np.linalg.eig(A) + print "modes \n", w + if print_modal_form: + # print "eigen vectors\n", M + # invM = np.linalg.inv(M) + # print "invM\n", invM + # Amod = np.dot(np.dot(invM, A), M) + # print "Amod\n", Amod + for i in range(len(w)): + print w[i], "->", M[:, i] diff --git a/sw/misc/attitude_reference/ressources/att_ref_gui.xml b/sw/misc/attitude_reference/ressources/att_ref_gui.xml new file mode 100644 index 0000000000..39bb73687e --- /dev/null +++ b/sw/misc/attitude_reference/ressources/att_ref_gui.xml @@ -0,0 +1,237 @@ + + + + + + False + KISS Attitude Reference Toy + + + True + False + vertical + 2 + + + True + False + 3 + + + True + False + 0 + in + + + True + False + 12 + + + True + False + 5 + + + True + False + 0 + Type + + + 0 + 0 + 1 + 1 + + + + + True + False + + + 1 + 0 + 2 + 1 + + + + + True + False + 0 + Duration + + + 0 + 1 + 1 + 1 + + + + + True + False + 0 + s + + + 2 + 1 + 1 + 1 + + + + + True + False + 0 + Step duration + + + 0 + 2 + 1 + 1 + + + + + True + False + 0 + s + + + 2 + 2 + 1 + 1 + + + + + True + False + 0 + Step amplitude + + + 0 + 3 + 1 + 1 + + + + + True + False + 0 + deg + + + 2 + 3 + 1 + 1 + + + + + True + True + 5 + ● + 1 + True + if-valid + + + 1 + 1 + 1 + 1 + + + + + True + True + 4 + ● + 1 + True + if-valid + + + 1 + 2 + 1 + 1 + + + + + True + True + 5 + ● + 1 + True + if-valid + + + 1 + 3 + 1 + 1 + + + + + + + + + True + False + <b>Setpoint</b> + True + + + + + False + True + 0 + + + + + + + + + + + + + + False + True + 0 + + + + + + + + + diff --git a/sw/misc/attitude_reference/ressources/att_ref_param_view.xml b/sw/misc/attitude_reference/ressources/att_ref_param_view.xml new file mode 100644 index 0000000000..5ce312536a --- /dev/null +++ b/sw/misc/attitude_reference/ressources/att_ref_param_view.xml @@ -0,0 +1,271 @@ + + + + + + False + + + True + False + + + True + False + 0 + Type: + + + 0 + 0 + 1 + 1 + + + + + True + False + + + 1 + 0 + 2 + 1 + + + + + True + False + 0 + Omega: + + + 0 + 1 + 1 + 1 + + + + + True + False + 0 + Xi: + + + 0 + 2 + 1 + 1 + + + + + True + False + 0 + MaxVel: + + + 0 + 3 + 1 + 1 + + + + + True + False + 0 + MaxAccel: + + + 0 + 4 + 1 + 1 + + + + + True + False + rad/s + + + 2 + 1 + 1 + 1 + + + + + True + False + deg/s + + + 2 + 3 + 1 + 1 + + + + + True + False + deg/s2 + + + 2 + 4 + 1 + 1 + + + + + True + False + deg/s3 + + + 2 + 5 + 1 + 1 + + + + + True + True + ● + number + 2 + True + if-valid + + + 1 + 1 + 1 + 1 + + + + + True + True + ● + digits + 2 + True + if-valid + + + 1 + 2 + 1 + 1 + + + + + True + True + ● + True + if-valid + + + 1 + 3 + 1 + 1 + + + + + True + True + ● + True + if-valid + + + 1 + 4 + 1 + 1 + + + + + True + True + ● + number + True + if-valid + + + 1 + 5 + 1 + 1 + + + + + True + False + + + 1 + 6 + 2 + 1 + + + + + True + False + 0 + MaxJerk: + + + 0 + 5 + 1 + 1 + + + + + True + False + 0 + Status: + + + 0 + 6 + 1 + 1 + + + + + + + + + diff --git a/sw/misc/attitude_reference/test_att_ref.py b/sw/misc/attitude_reference/test_att_ref.py new file mode 100755 index 0000000000..267cb23090 --- /dev/null +++ b/sw/misc/attitude_reference/test_att_ref.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# 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. +# + +import math +import numpy as np +import scipy.signal +import matplotlib.pyplot as plt + +import pat.utils as pu +import pat.algebra as pa + +import control as ctl + + +def random_setpoint(time, dt_step=2): + tf = time[0] + sp = np.zeros((len(time), 3)) + sp_i = [0, 0, 0] + for i in range(0, len(time)): + if time[i] >= tf: + ui = np.random.rand(3) - [0.5, 0.5, 0.5]; + ai = np.random.rand(1) + n = np.linalg.norm(ui) + if n > 0: + ui /= n + sp_i = pa.euler_of_quat(pa.quat_of_axis_angle(ui, ai)) + tf += dt_step + sp[i] = sp_i + return sp + + +def test_ref(r, time, setpoint): + ref = np.zeros((len(time), 9)) + for i in range(1, time.size): + sp_quat = pa.quat_of_euler(setpoint[i]) + r.update_quat(sp_quat, time[i] - time[i - 1]) + euler = pa.euler_of_quat(r.quat) + ref[i] = np.concatenate((euler, r.vel, r.accel)) + return ref + + +def plot_ref(time, xref=None, sp=None, figure=None): + margins = (0.05, 0.05, 0.98, 0.96, 0.20, 0.34) + figure = pu.prepare_fig(figure, window_title='Reference', figsize=(20.48, 10.24), margins=margins) + plots = [("$\phi$", "deg"), ("$\\theta$", "deg"), ("$\\psi$", "deg"), + ("$p$", "deg/s"), ("$q$", "deg/s"), ("$r$", "deg/s"), + ("$\dot{p}$", "deg/s2"), ("$\dot{q}$", "deg/s2"), ("$\dot{r}$", "deg/s2")] + for i, (title, ylab) in enumerate(plots): + ax = plt.subplot(3, 3, i + 1) + if xref is not None: plt.plot(time, pu.deg_of_rad(xref[:, i])) + pu.decorate(ax, title=title, ylab=ylab) + if sp is not None and i < 3: + plt.plot(time, pu.deg_of_rad(sp[:, i])) + return figure + + +dt = 1. / 512. +time = np.arange(0., 4, dt) +sp = np.zeros((len(time), 3)) +sp[:, 0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi) +# sp[:, 1] = pu.rad_of_deg(5.)*scipy.signal.square(math.pi/2*time) +# sp[:, 2] = pu.rad_of_deg(45.) +# sp = random_setpoint(time) + +# rs = [ctl.att_ref_analytic_disc(axis=0), ctl.att_ref_analytic_cont(axis=0), ctl.att_ref_default()] + +args = {'omega': 10., 'xi': 0.7, 'sat_vel': pu.rad_of_deg(150.), 'sat_accel': pu.rad_of_deg(1800), + 'sat_jerk': pu.rad_of_deg(27000)} +rs = [ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested(**args), ctl.att_ref_sat_nested2(**args)] +# beware that the saturation parameters of the native implementations are currently defined at compile time!! +# rs.append(ctl.AttRefIntNative(**args)) +rs.append(ctl.AttRefFloatNative(**args)) + +xrs = [test_ref(r, time, sp) for r in rs] +figure = None +for xr in xrs: + figure = plot_ref(time, xr, None, figure) +figure = plot_ref(time, None, sp, figure) +legends = [r.name for r in rs] + ['Setpoint'] +plt.subplot(3, 3, 3) +plt.legend(legends) +plt.show() diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml index 887bc0d21c..91b50e1901 100644 --- a/sw/supervision/paparazzicenter.ml +++ b/sw/supervision/paparazzicenter.ml @@ -210,7 +210,7 @@ let () = let ac_combo = AC.parse_conf_xml gui#vbox_ac and target_combo = Gtk_tools.combo ~width:50 ["sim";"fbw";"ap"] gui#vbox_target - and flash_combo = Gtk_tools.combo ~width:50 ["Default mode"] gui#vbox_flash in + and flash_combo = Gtk_tools.combo ~width:50 ["Default"] gui#vbox_flash in (Gtk_tools.combo_widget target_combo)#misc#set_sensitive false; (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml index 38b2a7d7f8..516cc196b1 100644 --- a/sw/supervision/pc_aircraft.ml +++ b/sw/supervision/pc_aircraft.ml @@ -228,7 +228,7 @@ let get_targets_list = fun ac_xml -> (** Parse Airframe File for Targets **) let parse_ac_targets = fun target_combo ac_file (log:string->unit) -> (* remember last target *) - let last_target = Gtk_tools.combo_value target_combo in + let last_target = try Gtk_tools.combo_value target_combo with _ -> "" in (* Clear ComboBox *) let (store, column) = Gtk_tools.combo_model target_combo in store#clear (); @@ -243,7 +243,9 @@ let parse_ac_targets = fun target_combo ac_file (log:string->unit) -> Gtk_tools.add_to_combo target_combo "sim" end; Gtk_tools.select_in_combo target_combo last_target - with _ -> log (sprintf "Error while parsing targets from file %s\n" ac_file) + with _ -> + log (sprintf "Error while parsing targets from file %s\n" ac_file); + raise Not_found (* Parse AC file for flash mode *) let parse_ac_flash = fun target flash_combo ac_file -> @@ -268,7 +270,8 @@ let parse_ac_flash = fun target flash_combo ac_file -> Gtk_tools.select_in_combo flash_combo last_flash_mode with _ -> (* not a valid airframe file *) - (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false + (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; + raise Not_found (* Link A/C to airframe & flight_plan labels *) let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo flash_combo (log:string->unit) -> @@ -301,8 +304,19 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo flash_com let aircraft = Hashtbl.find Utils.aircrafts ac_name in let sample = aircraft_sample ac_name "42" in (* update list of modules settings *) + let af_file = (Env.paparazzi_home // "conf" // (Xml.attrib aircraft "airframe")) in + let af_xml = try Xml.parse_file af_file + with + | Xml.File_not_found x -> + gui#label_airframe#set_text ""; + gui#button_clean#misc#set_sensitive false; + gui#button_build#misc#set_sensitive false; + (Gtk_tools.combo_widget target_combo)#misc#set_sensitive false; + (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; + log (sprintf "Error airframe file not found: %s\n" x); + Xml.Element ("airframe", [], []); + in let settings_modules = try - let af_xml = Xml.parse_file (Env.paparazzi_home // "conf" // (Xml.attrib aircraft "airframe")) in get_settings_modules af_xml (ExtXml.attrib_or_default aircraft "settings_modules" "") with | Failure x -> prerr_endline x; [] @@ -326,23 +340,33 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo flash_com and gui_color = ExtXml.attrib_or_default aircraft "gui_color" "white" in gui#button_clean#misc#set_sensitive true; gui#button_build#misc#set_sensitive true; + gui#button_upload#misc#set_sensitive true; gui#eventbox_gui_color#misc#modify_bg [`NORMAL, `NAME gui_color]; current_color := gui_color; gui#entry_ac_id#set_text ac_id; (Gtk_tools.combo_widget target_combo)#misc#set_sensitive true; (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive true; - let last_flash_mode = Gtk_tools.combo_value flash_combo in - parse_ac_targets target_combo (ExtXml.attrib aircraft "airframe") log; - parse_ac_flash (Gtk_tools.combo_value target_combo) flash_combo (ExtXml.attrib aircraft "airframe"); + let last_flash_mode = try Gtk_tools.combo_value flash_combo with _ -> "Default" in + begin + (* try parsing target from airframe file, may fail if not valid *) + try parse_ac_targets target_combo (ExtXml.attrib aircraft "airframe") log with _ -> + (Gtk_tools.combo_widget target_combo)#misc#set_sensitive false; + gui#button_build#misc#set_sensitive false + end; + begin + try parse_ac_flash (Gtk_tools.combo_value target_combo) flash_combo (ExtXml.attrib aircraft "airframe") with _ -> + (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; + gui#button_upload#misc#set_sensitive false + end; Gtk_tools.select_in_combo flash_combo last_flash_mode; with Not_found -> - gui#label_airframe#set_text ""; - gui#label_flight_plan#set_text ""; - gui#button_clean#misc#set_sensitive false; + (* Not found in aircrafts hashtbl *) gui#button_build#misc#set_sensitive false; + gui#button_clean#misc#set_sensitive false; (Gtk_tools.combo_widget target_combo)#misc#set_sensitive false; - (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false + (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; + log (sprintf "Aircraft %s not in conf\n" ac_name) in Gtk_tools.combo_connect ac_combo update_params;