[guidance_indi] lift effectiveness and set stabilization limits in WE…

…AK (#3117)

* [guidance_indi] lift effectiveness of the wing and weak function to set stabilization limits

Weak function to set indi_stab_dumin

* WEAK instead of ABI

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -130,6 +130,17 @@ static void guidance_indi_filter_thrust(void);
 #endif
 #endif
 
+#ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD
+#define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0
+#endif
+
+#ifndef GUIDANCE_INDI_DESCEND_SPEED_FWD
+#define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0
+#endif
+
+float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD;
+float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD;
+
 float inv_eff[4];
 
 // Max bank angle in radians
@@ -169,7 +180,6 @@ float time_of_vel_sp = 0.0;
 
 void guidance_indi_propagate_filters(void);
 static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
-static float guidance_indi_get_liftd(float pitch, float theta);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -530,7 +540,7 @@ static float bound_vz_sp(float vz_sp)
 {
   // Bound vertical speed setpoint
   if (stateGetAirspeed_f() > 13.f) {
-    Bound(vz_sp, -4.0f, 4.0f); // FIXME no harcoded values
+    Bound(vz_sp, -climb_vspeed_fwd, -descend_vspeed_fwd);
   } else {
     Bound(vz_sp, -nav.climb_vspeed, -nav.descend_vspeed); // FIXME don't use nav settings
   }
@@ -695,7 +705,7 @@ void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {
  *
  * @return The derivative of lift w.r.t. pitch
  */
-float guidance_indi_get_liftd(float airspeed, float theta) {
+float WEAK guidance_indi_get_liftd(float airspeed, float theta) {
   float liftd = 0.0;
 
   if(airspeed < 12) {

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h

@@ -41,6 +41,7 @@
 
 extern void guidance_indi_init(void);
 extern void guidance_indi_enter(void);
+extern float guidance_indi_get_liftd(float pitch, float theta);
 
 enum GuidanceIndiHybrid_HMode {
   GUIDANCE_INDI_HYBRID_H_POS,

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

@@ -81,9 +81,13 @@
 #define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0
 #endif
 
-float du_min[INDI_NUM_ACT];
-float du_max[INDI_NUM_ACT];
-float du_pref[INDI_NUM_ACT];
+#ifdef SetCommandsFromRC
+#warning SetAutoCommandsFromRC not used: STAB_INDI overwrites actuators
+#endif
+
+float du_min_stab_indi[INDI_NUM_ACT];
+float du_max_stab_indi[INDI_NUM_ACT];
+float du_pref_stab_indi[INDI_NUM_ACT];
 float indi_v[INDI_OUTPUTS];
 float *Bwls[INDI_OUTPUTS];
 int num_iter = 0;
@@ -577,30 +581,12 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
       + (g1g2_pseudo_inv[i][3] * indi_v[3]);
   }
 #else
-  // Calculate the min and max increments
-  for (i = 0; i < INDI_NUM_ACT; i++) {
-    du_min[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
-    du_max[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
-    du_pref[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
+  stabilization_indi_set_wls_settings(use_increment);
 
-#ifdef GUIDANCE_INDI_MIN_THROTTLE
-    float airspeed = stateGetAirspeed_f();
-    //limit minimum thrust ap can give
-    if (!act_is_servo[i]) {
-      if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
-        if (airspeed < INDI_HROTTLE_LIMIT_AIRSPEED_FWD) {
-          du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment*actuator_state_filt_vect[i];
-        } else {
-          du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment*actuator_state_filt_vect[i];
-        }
-      }
-    }
-#endif
-  }
 
   // WLS Control Allocator
   num_iter =
-    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, du_pref, 10000, 10);
+    wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10);
 #endif
 
   if (in_flight) {
@@ -655,6 +641,35 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   stabilization_cmd[COMMAND_YAW] = 42;
 }
 
+/**
+ * @param use_increment 
+ * 
+ * Function that sets the du_min, du_max and du_pref if function not elsewhere defined
+ */
+void WEAK stabilization_indi_set_wls_settings(float use_increment)
+{
+   // Calculate the min and max increments
+    for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
+      du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
+      du_max_stab_indi[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
+      du_pref_stab_indi[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
+
+#ifdef GUIDANCE_INDI_MIN_THROTTLE
+    float airspeed = stateGetAirspeed_f();
+    //limit minimum thrust ap can give
+    if (!act_is_servo[i]) {
+      if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
+        if (airspeed < INDI_HROTTLE_LIMIT_AIRSPEED_FWD) {
+          du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment*actuator_state_filt_vect[i];
+        } else {
+          du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment*actuator_state_filt_vect[i];
+        }
+      }
+    }
+#endif
+  }
+}
+
 /**
  * @param enable_integrator
  * @param rate_control boolean that determines if we are in rate control or attitude control

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

@@ -37,6 +37,9 @@ extern float actuator_state_filt_vect[INDI_NUM_ACT];
 
 extern bool indi_use_adaptive;
 
+extern float du_min_stab_indi[INDI_NUM_ACT];
+extern float du_max_stab_indi[INDI_NUM_ACT];
+extern float du_pref_stab_indi[INDI_NUM_ACT];
 extern float *Bwls[INDI_OUTPUTS];
 
 extern float act_pref[INDI_NUM_ACT];
@@ -60,6 +63,7 @@ extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
 extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
 extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
 extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
+extern void stabilization_indi_set_wls_settings(float use_increment);
 extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
 extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);