[rotorcraft] subsystem for stabilization float_quat and some defines,…

… etc.

conf/autopilot/subsystems/rotorcraft/stabilization_float_quat.makefile

@@ -0,0 +1,5 @@
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_float.h\"
+ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
+ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
+ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c

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

@@ -28,6 +28,14 @@
 
 #include "generated/airframe.h"
 
+#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_NB
+#define STABILIZATION_ATTITUDE_FLOAT_GAIN_NB 1
+#endif
+
+#ifndef STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT
+#define STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT 0
+#endif
+
 struct FloatAttitudeGains {
   struct FloatVect3  p;
   struct FloatVect3  d;

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

@@ -32,22 +32,11 @@
 #include "math/pprz_algebra_int.h"
 #include "subsystems/ahrs.h"
 #include "generated/airframe.h"
+#include "stabilization_attitude_float.h"
+#include "stabilization_attitude_rc_setpoint.h"
 
 struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB];
 
-/* warn if some gains are still negative */
-#if (STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN < 0) ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN < 0) ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN < 0)   ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN < 0)   ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN < 0) ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN < 0)   ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN < 0)   ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN < 0) ||   \
-  (STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN  < 0)
-#warning "ALL control gains are now positive!!!"
-#endif
-
 struct FloatQuat stabilization_att_sum_err_quat;
 struct FloatEulers stabilization_att_sum_err_eulers;
 
@@ -132,6 +121,9 @@ void stabilization_attitude_enter(void) {
   FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
 }
 
+#ifndef GAIN_PRESCALER_FF
+#define GAIN_PRESCALER_FF 1
+#endif
 static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
 {
   /* Compute feedforward based on reference acceleration */
@@ -144,6 +136,15 @@ static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gain
   ff_commands[COMMAND_YAW_SURFACE]   = GAIN_PRESCALER_FF * gains->surface_dd.z * ref_accel->r;
 }
 
+#ifndef GAIN_PRESCALER_P
+#define GAIN_PRESCALER_P 1
+#endif
+#ifndef GAIN_PRESCALER_D
+#define GAIN_PRESCALER_D 1
+#endif
+#ifndef GAIN_PRESCALER_I
+#define GAIN_PRESCALER_I 1
+#endif
 static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err,
     struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
 {
@@ -230,4 +231,15 @@ void stabilization_attitude_run(bool_t enable_integrator) {
   for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
     stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
   }
+
+  /* bound the result */
+  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
+  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
+  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
+}
+
+void stabilization_attitude_read_rc(bool_t in_flight) {
+
+  stabilization_attitude_read_rc_setpoint_quat_float(&stab_att_sp_quat, in_flight);
+  //FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
 }

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

@@ -26,19 +26,44 @@
 #ifndef STABILISATION_ATTITUDE_RC_SETPOINT_H
 #define STABILISATION_ATTITUDE_RC_SETPOINT_H
 
+#include "std.h"
 #include "generated/airframe.h"
 #include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
 
 #include "subsystems/radio_control.h"
 #include "subsystems/ahrs.h"
 
+#ifdef STABILISATION_ATTITUDE_TYPE_INT
+#define SP_MAX_PHI     (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
+#define SP_MAX_THETA   (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA)
+#define SP_MAX_R       (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R)
+#endif // STABILISATION_ATTITUDE_TYPE_INT
 
-#define SP_MAX_PHI   (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
-#define SP_MAX_THETA (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA)
-#define SP_MAX_R     (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R)
+#ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
+#define SP_MAX_PHI   STABILIZATION_ATTITUDE_SP_MAX_PHI
+#define SP_MAX_THETA STABILIZATION_ATTITUDE_SP_MAX_THETA
+#define SP_MAX_R     STABILIZATION_ATTITUDE_SP_MAX_R
+#endif // STABILISATION_ATTITUDE_TYPE_FLOAT
 
 #define RC_UPDATE_FREQ 40
 
+#ifdef STABILIZATION_ATTITUDE_DEADBAND_A
+#define ROLL_DEADBAND_EXCEEDED()                                        \
+  (radio_control.values[RADIO_ROLL] >  STABILIZATION_ATTITUDE_DEADBAND_A || \
+   radio_control.values[RADIO_ROLL] < -STABILIZATION_ATTITUDE_DEADBAND_A)
+#else
+#define ROLL_DEADBAND_EXCEEDED() (TRUE)
+#endif /* STABILIZATION_ATTITUDE_DEADBAND_A */
+
+#ifdef STABILIZATION_ATTITUDE_DEADBAND_E
+#define PITCH_DEADBAND_EXCEEDED()                                       \
+  (radio_control.values[RADIO_PITCH] >  STABILIZATION_ATTITUDE_DEADBAND_E || \
+   radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E)
+#else
+#define PITCH_DEADBAND_EXCEEDED() (TRUE)
+#endif /* STABILIZATION_ATTITUDE_DEADBAND_E */
+
 #define YAW_DEADBAND_EXCEEDED()                                         \
   (radio_control.values[RADIO_YAW] >  STABILIZATION_ATTITUDE_DEADBAND_R || \
    radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)

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

@@ -28,27 +28,9 @@
 
 #include "stabilization_attitude_ref_float.h"
 
-#define RC_UPDATE_FREQ 40.
-#define ROLL_COEF   (STABILIZATION_ATTITUDE_FLOAT_SP_MAX_PHI   / MAX_PPRZ)
-// FIXME: unused, what was it supposed to be?
-//#define ROLL_COEF_H (STABILIZATION_ATTITUDE_FLOAT_SP_MAX_P_H   / MAX_PPRZ)
-#define PITCH_COEF  (STABILIZATION_ATTITUDE_FLOAT_SP_MAX_THETA / MAX_PPRZ)
-// FIXME: what is this supposed to be??
-#define YAW_COEF    (STABILIZATION_ATTITUDE_FLOAT_SP_MAX_PSI   / MAX_PPRZ)
-
 #define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE))
 #define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0)
 
-#define ROLL_DEADBAND_EXCEEDED()						\
-  (radio_control.values[RADIO_ROLL] >  STABILIZATION_ATTITUDE_FLOAT_DEADBAND_A || \
-   radio_control.values[RADIO_ROLL] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_A)
-#define PITCH_DEADBAND_EXCEEDED()						\
-  (radio_control.values[RADIO_PITCH] >  STABILIZATION_ATTITUDE_FLOAT_DEADBAND_E || \
-   radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_E)
-#define YAW_DEADBAND_EXCEEDED()						\
-  (radio_control.values[RADIO_YAW] >  STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R || \
-   radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R)
-
 void stabilization_attitude_ref_enter(void);
 void stabilization_attitude_ref_schedule(uint8_t idx);