[rotorcraft] att_ref_quat_float: runtime adjustable saturation

and default saturation values

sw/airborne/firmwares/rotorcraft/stabilization/attitude_ref_saturate_naive.h

@@ -0,0 +1,66 @@
+/*
+ * 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_saturate_naive.h
+ *  Naive attitude reference saturation.
+ */
+
+#ifndef ATTITUDE_REF_SATURATE_NAIVE_H
+#define ATTITUDE_REF_SATURATE_NAIVE_H
+
+#include "math/pprz_algebra_float.h"
+
+struct FloatRefSat {
+  struct FloatRates max_rate;
+  struct FloatRates 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.;
+    }
+  }
+  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,
+                                                     struct FloatRefSat *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 */

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c

@@ -30,21 +30,31 @@
 #include "generated/airframe.h"
 
 #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 */
+#ifndef STABILIZATION_ATTITUDE_REF_MAX_P
-#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
+#define STABILIZATION_ATTITUDE_REF_MAX_P RadOfDeg(400.)
-#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
+#endif
-#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
+#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
 
-#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
+#ifndef STABILIZATION_ATTITUDE_REF_MAX_PDOT
-#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
+#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(2000.)
-#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
+#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 */
@@ -70,6 +80,13 @@ void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
   FLOAT_RATES_ZERO(ref->rate);
   FLOAT_RATES_ZERO(ref->accel);
 
+  ref->saturation.max_rate.p = STABILIZATION_ATTITUDE_REF_MAX_P;
+  ref->saturation.max_rate.q = STABILIZATION_ATTITUDE_REF_MAX_Q;
+  ref->saturation.max_rate.r = STABILIZATION_ATTITUDE_REF_MAX_R;
+  ref->saturation.max_accel.p = STABILIZATION_ATTITUDE_REF_MAX_PDOT;
+  ref->saturation.max_accel.q = STABILIZATION_ATTITUDE_REF_MAX_QDOT;
+  ref->saturation.max_accel.r = STABILIZATION_ATTITUDE_REF_MAX_RDOT;
+
   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]);
@@ -124,13 +141,8 @@ void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQua
   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 */
+  /* saturate */
-  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
+  attitude_ref_float_saturate_naive(&ref->rate, &ref->accel, &ref->saturation);
-  const struct FloatRates 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);
 
   /* compute ref_euler */
   float_eulers_of_quat(&ref->euler, &ref->quat);

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h

@@ -32,6 +32,7 @@
 
 #include "generated/airframe.h"
 #include "math/pprz_algebra_float.h"
+#include "attitude_ref_saturate_naive.h"
 
 #ifndef STABILIZATION_ATTITUDE_GAIN_NB
 #define STABILIZATION_ATTITUDE_GAIN_NB 1
@@ -53,6 +54,7 @@ struct AttRefQuatFloat {
   struct FloatRates  accel;
   uint8_t cur_idx;
   struct FloatRefModel  model[STABILIZATION_ATTITUDE_GAIN_NB];
+  struct FloatRefSat saturation;
 };
 
 

sw/airborne/math/pprz_algebra.h

@@ -409,6 +409,12 @@ extern "C" {
     if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
   }
 
+#define RATES_BOUND_BOX_ABS(_v, _v_max) {       \
+    if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < -(_v_max).p) (_v).p = -(_v_max).p; \
+    if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < -(_v_max).q) (_v).q = -(_v_max).q; \
+    if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < -(_v_max).r) (_v).r = -(_v_max).r; \
+  }
+
 #define RATES_ADD_SCALED_VECT(_ro, _v, _s) {    \
     (_ro).p += (_v).x * (_s);                   \
     (_ro).q += (_v).y * (_s);                   \

sw/misc/attitude_reference/c_att_refs/generated/airframe.h

@@ -9,16 +9,12 @@
 
 #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
 

sw/misc/attitude_reference/c_att_refs/ref_quat_float.pxd

@@ -5,12 +5,17 @@ cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
         FloatRates omega
         FloatRates zeta
 
+    struct FloatRefSat:
+        FloatRates max_rate
+        FloatRates max_accel
+
     struct AttRefQuatFloat:
         FloatEulers euler
         FloatQuat   quat
         FloatRates  rate
         FloatRates  accel
         FloatRefModel model[1]
+        FloatRefSat saturation
 
     void attitude_ref_quat_float_init(AttRefQuatFloat *ref)
     void attitude_ref_quat_float_enter(AttRefQuatFloat *ref, float psi)

sw/misc/attitude_reference/c_att_refs/ref_quat_float.pyx

@@ -78,3 +78,23 @@ cdef class RefQuatFloat:
                 self.set_omega(omega * np.ones(3))
             else:
                 self.set_omega(omega)
+
+    property sat_vel:
+        def __get__(self):
+            return np.array([self.ref.saturation.max_rate.p, self.ref.saturation.max_rate.q, self.ref.saturation.max_rate.r])
+        def __set__(self, vel):
+            if type(vel) == float:
+                vel = vel * np.ones(3)
+            self.ref.saturation.max_rate.p = vel[0]
+            self.ref.saturation.max_rate.q = vel[1]
+            self.ref.saturation.max_rate.r = vel[2]
+
+    property sat_accel:
+        def __get__(self):
+            return np.array([self.ref.saturation.max_accel.p, self.ref.saturation.max_accel.q, self.ref.saturation.max_accel.r])
+        def __set__(self, accel):
+            if type(accel) == float:
+                accel = accel * np.ones(3)
+            self.ref.saturation.max_accel.p = accel[0]
+            self.ref.saturation.max_accel.q = accel[1]
+            self.ref.saturation.max_accel.r = accel[2]

sw/misc/attitude_reference/control.py

@@ -45,6 +45,9 @@ class AttitudeReference(object):
         self.omega = kwargs.get('omega', 6.)
         self.xi = kwargs.get('xi', 0.8)
         self.t = kwargs.get('t0', 0.)
+        for key, value in kwargs.iteritems():
+            if hasattr(self, key):
+                setattr(self, key, value)
         for p in ['omega', 'xi']:
             self.ensure_vect(p)
 

sw/misc/attitude_reference/gui.py

@@ -77,8 +77,8 @@ class AttRefParamView(Gtk.Frame):
         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_vel': {'range': (1., 500., 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_accel': {'range': (10., 2000., 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():

sw/misc/attitude_reference/test_att_ref.py

@@ -87,7 +87,7 @@ 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 the native implementations are currently defined at compile time!!
+# 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))