[rotorcraft][stabilization_ref_quat] saturate rate and trim accel

paparazzi · Aug 3, 2013 · 2061fed · 2061fed
1 parent 8bce6d4
commit 2061fed
Show file tree

Hide file tree

Showing 7 changed files with 19 additions and 7 deletions.
diff --git a/...zation/stabilization_attitude_ref_euler.h → ...tabilization/stabilization_attitude_ref.h b/...zation/stabilization_attitude_ref_euler.h → ...tabilization/stabilization_attitude_ref.h
@@ -19,12 +19,12 @@
  * Boston, MA 02111-1307, USA.
  */
 
-/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
- *  Common rotorcraft attitude euler reference generation include.
+/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
+ *  Common rotorcraft attitude reference generation include.
  */
 
-#ifndef STABILIZATION_ATTITUDE_REF_EULER_H
-#define STABILIZATION_ATTITUDE_REF_EULER_H
+#ifndef STABILIZATION_ATTITUDE_REF_H
+#define STABILIZATION_ATTITUDE_REF_H
 
 #define SATURATE_SPEED_TRIM_ACCEL() {                   \
     if (stab_att_ref_rate.p >= REF_RATE_MAX_P) {		\
@@ -60,4 +60,4 @@
   }
 
 
-#endif /* STABILIZATION_ATTITUDE_REF_EULER_H */
+#endif /* STABILIZATION_ATTITUDE_REF_H */
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
@@ -25,7 +25,7 @@
 #include "math/pprz_algebra_float.h"
 
 #include "stabilization_attitude_ref_float.h"
-#include "stabilization_attitude_ref_euler.h"
+#include "stabilization_attitude_ref.h"
 
 void stabilization_attitude_ref_enter(void);
 

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
@@ -28,7 +28,7 @@
 #define STABILIZATION_ATTITUDE_REF_EULER_INT_H
 
 #include "stabilization_attitude_ref_int.h"
-#include "stabilization_attitude_ref_euler.h"
+#include "stabilization_attitude_ref.h"
 
 
 #endif /* STABILIZATION_ATTITUDE_REF_EULER_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
@@ -37,6 +37,10 @@
 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
 
+#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
+#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;
@@ -146,6 +150,9 @@ void stabilization_attitude_ref_update(void) {
   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);
 
+  /* saturate angular speed and trim accel accordingly */
+  SATURATE_SPEED_TRIM_ACCEL();
+
   /* compute ref_euler */
   FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
 }
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
@@ -34,6 +34,7 @@
 #include "math/pprz_algebra_float.h"
 
 #include "stabilization_attitude_ref_float.h"
+#include "stabilization_attitude_ref.h"
 
 #define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE))
 #define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0)

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
@@ -178,6 +178,9 @@ void stabilization_attitude_ref_update(void) {
   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);
 
+  /* saturate angular speed and trim accel accordingly */
+  SATURATE_SPEED_TRIM_ACCEL();
+
 
   /* compute ref_euler for debugging and telemetry */
   struct Int32Eulers ref_eul;

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
@@ -31,6 +31,7 @@
 #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
 
 #include "stabilization_attitude_ref_int.h"
+#include "stabilization_attitude_ref.h"
 
 void stabilization_attitude_ref_enter(void);