[rotorcraft] att_ref_quat_float: runtime adjustable saturation

and default saturation values

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_accel': {'range': (10., 1500., 10., 20., 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., 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))