diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/attitude_ref_saturate_naive.h b/sw/airborne/firmwares/rotorcraft/stabilization/attitude_ref_saturate_naive.h index 2f8a956692..563f94f0df 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/attitude_ref_saturate_naive.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/attitude_ref_saturate_naive.h @@ -27,28 +27,33 @@ #define ATTITUDE_REF_SATURATE_NAIVE_H #include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" struct FloatRefSat { struct FloatRates max_rate; struct FloatRates max_accel; }; +struct Int32RefSat { + struct Int32Rates max_rate; + struct Int32Rates max_accel; +}; + /** saturate angular speed and trim accel accordingly */ -static inline void saturate_speed_trim_accel(float *rate, float *accel, float max_rate) -{ - if (*rate >= max_rate) { - *rate = max_rate; - if (*accel > 0.) { - *accel = 0.; - } +#define SATURATE_SPEED_TRIM_ACCEL(_rate, _accel, _max_rate) { \ + if ((_rate) >= (_max_rate)) { \ + (_rate) = (_max_rate); \ + if ((_accel) > 0) { \ + (_accel) = 0; \ + } \ + } \ + else if ((_rate) <= -(_max_rate)) { \ + (_rate) = -(_max_rate); \ + if ((_accel) < 0) { \ + (_accel) = 0; \ + } \ + } \ } - else if (*rate <= -max_rate) { - *rate = -max_rate; - if (*accel < 0.) { - *accel = 0.; - } - } -} static inline void attitude_ref_float_saturate_naive(struct FloatRates *rate, struct FloatRates *accel, @@ -58,9 +63,22 @@ static inline void attitude_ref_float_saturate_naive(struct FloatRates *rate, RATES_BOUND_BOX_ABS(*accel, sat->max_accel); /* saturate angular speed and trim accel accordingly */ - saturate_speed_trim_accel(&rate->p, &accel->p, sat->max_rate.p); - saturate_speed_trim_accel(&rate->q, &accel->q, sat->max_rate.q); - saturate_speed_trim_accel(&rate->r, &accel->r, sat->max_rate.r); + SATURATE_SPEED_TRIM_ACCEL(rate->p, accel->p, sat->max_rate.p); + SATURATE_SPEED_TRIM_ACCEL(rate->q, accel->q, sat->max_rate.q); + SATURATE_SPEED_TRIM_ACCEL(rate->r, accel->r, sat->max_rate.r); +} + +static inline void attitude_ref_int_saturate_naive(struct Int32Rates *rate, + struct Int32Rates *accel, + struct Int32RefSat *sat) +{ + /* saturate angular acceleration */ + RATES_BOUND_BOX_ABS(*accel, sat->max_accel); + + /* saturate angular speed and trim accel accordingly */ + SATURATE_SPEED_TRIM_ACCEL(rate->p, accel->p, sat->max_rate.p); + SATURATE_SPEED_TRIM_ACCEL(rate->q, accel->q, sat->max_rate.q); + SATURATE_SPEED_TRIM_ACCEL(rate->r, accel->r, sat->max_rate.r); } #endif /* ATTITUDE_REF_SATURATE_NAIVE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h new file mode 100644 index 0000000000..fafdf1a90a --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2008-2015 The Paparazzi Team + * + * 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 firmwares/rotorcraft/stabilization/attitude_ref_defaults.h + * Default values for attitude reference saturation. + */ + +#ifndef ATTITUDE_REF_DEFAULTS_H +#define ATTITUDE_REF_DEFAULTS_H + +#ifndef STABILIZATION_ATTITUDE_REF_MAX_P +#define STABILIZATION_ATTITUDE_REF_MAX_P RadOfDeg(400.) +#endif +#ifndef STABILIZATION_ATTITUDE_REF_MAX_Q +#define STABILIZATION_ATTITUDE_REF_MAX_Q RadOfDeg(400.) +#endif +#ifndef STABILIZATION_ATTITUDE_REF_MAX_R +#define STABILIZATION_ATTITUDE_REF_MAX_R RadOfDeg(180.) +#endif + +#ifndef STABILIZATION_ATTITUDE_REF_MAX_PDOT +#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(2000.) +#endif +#ifndef STABILIZATION_ATTITUDE_REF_MAX_QDOT +#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(2000.) +#endif +#ifndef STABILIZATION_ATTITUDE_REF_MAX_RDOT +#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.) +#endif + +#endif /* ATTITUDE_REF_DEFAULTS_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 82e911889e..8eec993a3d 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 @@ -28,27 +28,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" - - -#ifndef STABILIZATION_ATTITUDE_REF_MAX_P -#define STABILIZATION_ATTITUDE_REF_MAX_P RadOfDeg(400.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_Q -#define STABILIZATION_ATTITUDE_REF_MAX_Q RadOfDeg(400.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_R -#define STABILIZATION_ATTITUDE_REF_MAX_R RadOfDeg(180.) -#endif - -#ifndef STABILIZATION_ATTITUDE_REF_MAX_PDOT -#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(2000.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_QDOT -#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(2000.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_RDOT -#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.) -#endif +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h" static inline void reset_psi_ref(struct AttRefEulerFloat *ref, float psi); 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 d8df656717..1994bc8a46 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 @@ -30,32 +30,13 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.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 -#ifndef STABILIZATION_ATTITUDE_REF_MAX_P -#define STABILIZATION_ATTITUDE_REF_MAX_P RadOfDeg(400.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_Q -#define STABILIZATION_ATTITUDE_REF_MAX_Q RadOfDeg(400.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_R -#define STABILIZATION_ATTITUDE_REF_MAX_R RadOfDeg(180.) -#endif - -#ifndef STABILIZATION_ATTITUDE_REF_MAX_PDOT -#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(2000.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_QDOT -#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(2000.) -#endif -#ifndef STABILIZATION_ATTITUDE_REF_MAX_RDOT -#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.) -#endif - /* parameters used for initialization */ static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P; 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 57da453133..afe3d24558 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 @@ -29,16 +29,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" -#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" - -#define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) -#define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) -#define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC) - -#define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC) -#define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC) -#define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC) - +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h" #define TWO_ZETA_OMEGA_RES 10 @@ -64,6 +55,13 @@ void attitude_ref_quat_int_init(struct AttRefQuatInt *ref) INT_RATES_ZERO(ref->rate); INT_RATES_ZERO(ref->accel); + attitude_ref_quat_int_set_max_p(ref, STABILIZATION_ATTITUDE_REF_MAX_P); + attitude_ref_quat_int_set_max_q(ref, STABILIZATION_ATTITUDE_REF_MAX_Q); + attitude_ref_quat_int_set_max_r(ref, STABILIZATION_ATTITUDE_REF_MAX_R); + attitude_ref_quat_int_set_max_pdot(ref, STABILIZATION_ATTITUDE_REF_MAX_PDOT); + attitude_ref_quat_int_set_max_qdot(ref, STABILIZATION_ATTITUDE_REF_MAX_QDOT); + attitude_ref_quat_int_set_max_rdot(ref, STABILIZATION_ATTITUDE_REF_MAX_RDOT); + struct FloatRates omega0 = {STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R}; @@ -153,22 +151,26 @@ void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *s 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(ref->accel, MIN_ACCEL, MAX_ACCEL); - - /* saturate angular speed and trim accel accordingly */ - SATURATE_SPEED_TRIM_ACCEL(*ref); - + /* saturate */ + attitude_ref_int_saturate_naive(&ref->rate, &ref->accel, &ref->saturation); /* compute euler representation for debugging and telemetry */ int32_eulers_of_quat(&ref->euler, &ref->quat); } +static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi) +{ + ref->euler.psi = psi; + ref->rate.r = 0; + ref->accel.r = 0; +} +/* + * Recomputation of cached values. + * + */ 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); @@ -195,6 +197,10 @@ static void update_ref_model(struct AttRefQuatInt *ref) } +/* + * Setting handlers for changing the ref model parameters. + * + */ void attitude_ref_quat_int_set_omega_p(struct AttRefQuatInt *ref, float omega_p) { ref->model.omega.p = omega_p; @@ -245,10 +251,33 @@ void attitude_ref_quat_int_set_zeta(struct AttRefQuatInt *ref, struct FloatRates attitude_ref_quat_int_set_zeta_r(ref, zeta->r); } - -static inline void reset_psi_ref(struct AttRefQuatInt *ref, int32_t psi) +void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p) { - ref->euler.psi = psi; - ref->rate.r = 0; - ref->accel.r = 0; + ref->saturation.max_rate.p = BFP_OF_REAL(max_p, REF_RATE_FRAC); } + +void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q) +{ + ref->saturation.max_rate.q = BFP_OF_REAL(max_q, REF_RATE_FRAC); +} + +void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r) +{ + ref->saturation.max_rate.r = BFP_OF_REAL(max_r, REF_RATE_FRAC); +} + +void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot) +{ + ref->saturation.max_accel.p = BFP_OF_REAL(max_pdot, REF_ACCEL_FRAC); +} + +void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot) +{ + ref->saturation.max_accel.q = BFP_OF_REAL(max_qdot, REF_ACCEL_FRAC); +} + +void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot) +{ + ref->saturation.max_accel.r = BFP_OF_REAL(max_rdot, REF_ACCEL_FRAC); +} + 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 20612318ad..caf397eddd 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 @@ -31,12 +31,13 @@ #define STABILIZATION_ATTITUDE_REF_QUAT_INT_H #include "stabilization_attitude_ref_int.h" +#include "attitude_ref_saturate_naive.h" /* 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 IntRefModel { struct FloatRates omega; struct FloatRates zeta; /* cached intermediate values in int */ @@ -50,7 +51,8 @@ struct AttRefQuatInt { struct Int32Quat quat; struct Int32Rates rate; ///< with #REF_RATE_FRAC struct Int32Rates accel; ///< with #REF_ACCEL_FRAC - struct FloatRefModel model; + struct IntRefModel model; + struct Int32RefSat saturation; }; extern void attitude_ref_quat_int_init(struct AttRefQuatInt *ref); @@ -67,4 +69,11 @@ extern void attitude_ref_quat_int_set_zeta_p(struct AttRefQuatInt *ref, float ze 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); +extern void attitude_ref_quat_int_set_max_p(struct AttRefQuatInt *ref, float max_p); +extern void attitude_ref_quat_int_set_max_q(struct AttRefQuatInt *ref, float max_q); +extern void attitude_ref_quat_int_set_max_r(struct AttRefQuatInt *ref, float max_r); +extern void attitude_ref_quat_int_set_max_pdot(struct AttRefQuatInt *ref, float max_pdot); +extern void attitude_ref_quat_int_set_max_qdot(struct AttRefQuatInt *ref, float max_qdot); +extern void attitude_ref_quat_int_set_max_rdot(struct AttRefQuatInt *ref, float max_rdot); + #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 deleted file mode 100644 index 3452022552..0000000000 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright (C) 2008-2010 The Paparazzi Team - * - * 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 firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h - * Common rotorcraft attitude reference saturation include. - */ - -#ifndef STABILIZATION_ATTITUDE_REF_SATURATE_H -#define STABILIZATION_ATTITUDE_REF_SATURATE_H - -#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; \ - } \ - } - - -#endif /* STABILIZATION_ATTITUDE_REF_SATURATE_H */ diff --git a/sw/misc/attitude_reference/c_att_refs/generated/airframe.h b/sw/misc/attitude_reference/c_att_refs/generated/airframe.h index 789350c84d..4425fbc217 100644 --- a/sw/misc/attitude_reference/c_att_refs/generated/airframe.h +++ b/sw/misc/attitude_reference/c_att_refs/generated/airframe.h @@ -20,16 +20,12 @@ #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 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 index 30aaa5a254..9c193d98e0 100644 --- a/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pxd @@ -4,16 +4,21 @@ 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: + struct IntRefModel: FloatRates omega FloatRates zeta + struct Int32RefSat: + Int32Rates max_rate + Int32Rates max_accel + struct AttRefQuatInt: Int32Eulers euler Int32Quat quat Int32Rates rate Int32Rates accel - FloatRefModel model + IntRefModel model + Int32RefSat saturation void attitude_ref_quat_int_init(AttRefQuatInt *ref) void attitude_ref_quat_int_enter(AttRefQuatInt *ref, int32_t psi) @@ -21,3 +26,10 @@ cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h": void attitude_ref_quat_int_set_omega(AttRefQuatInt *ref, FloatRates *omega) void attitude_ref_quat_int_set_zeta(AttRefQuatInt *ref, FloatRates *zeta) + + void attitude_ref_quat_int_set_max_p(AttRefQuatInt *ref, float max_p); + void attitude_ref_quat_int_set_max_q(AttRefQuatInt *ref, float max_q); + void attitude_ref_quat_int_set_max_r(AttRefQuatInt *ref, float max_r); + void attitude_ref_quat_int_set_max_pdot(AttRefQuatInt *ref, float max_pdot); + void attitude_ref_quat_int_set_max_qdot(AttRefQuatInt *ref, float max_qdot); + void attitude_ref_quat_int_set_max_rdot(AttRefQuatInt *ref, float max_rdot); 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 index 100e80bf7e..2a4c88e596 100644 --- a/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx +++ b/sw/misc/attitude_reference/c_att_refs/ref_quat_int.pyx @@ -97,3 +97,25 @@ cdef class RefQuatInt: self.set_omega(omega * np.ones(3)) else: self.set_omega(omega) + + property sat_vel: + def __get__(self): + max_rate = [self.ref.saturation.max_rate.p, self.ref.saturation.max_rate.q, self.ref.saturation.max_rate.r] + return np.array(max_rate, dtype='d') / (1 << REF_RATE_FRAC) + def __set__(self, vel): + if type(vel) == float: + vel = vel * np.ones(3) + ref_quat_int.attitude_ref_quat_int_set_max_p(&self.ref, vel[0]) + ref_quat_int.attitude_ref_quat_int_set_max_q(&self.ref, vel[1]) + ref_quat_int.attitude_ref_quat_int_set_max_r(&self.ref, vel[2]) + + property sat_accel: + def __get__(self): + max_accel = [self.ref.saturation.max_accel.p, self.ref.saturation.max_accel.q, self.ref.saturation.max_accel.r] + return np.array(max_accel, dtype='d') / (1 << REF_ACCEL_FRAC) + def __set__(self, accel): + if type(accel) == float: + accel = accel * np.ones(3) + ref_quat_int.attitude_ref_quat_int_set_max_pdot(&self.ref, accel[0]) + ref_quat_int.attitude_ref_quat_int_set_max_qdot(&self.ref, accel[1]) + ref_quat_int.attitude_ref_quat_int_set_max_rdot(&self.ref, accel[2]) diff --git a/sw/misc/attitude_reference/test_att_ref.py b/sw/misc/attitude_reference/test_att_ref.py index ce07946fce..45d1bd34b8 100755 --- a/sw/misc/attitude_reference/test_att_ref.py +++ b/sw/misc/attitude_reference/test_att_ref.py @@ -87,7 +87,6 @@ sp[:, 0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi 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 IntNative can only be set/defined at compile time!! # rs.append(ctl.AttRefIntNative(**args)) rs.append(ctl.AttRefFloatNative(**args))