Rotwing control update made compatible with master (#3374)

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

@@ -71,6 +71,42 @@
 #define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
 #endif
 
+#ifndef GUIDANCE_INDI_MAX_AIRSPEED
+#error "You must have an airspeed sensor to use this guidance"
+#endif
+
+#ifndef GUIDANCE_INDI_MIN_AIRSPEED
+#define GUIDANCE_INDI_MIN_AIRSPEED -10.f
+#endif
+
+/**
+ * Climb speed when navigation is making turns instead of direct lines
+ */
+#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED
+#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0
+#endif
+
+/**
+ * Descend speed when navigation is making turns instead of direct lines
+ */
+#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED
+#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0
+#endif
+
+/**
+ * Climb speed when navigation is doing direct lines
+ */
+#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED
+#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0
+#endif
+
+/**
+ * Descend speed when navigation is doing direct lines
+ */
+#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED
+#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0
+#endif
+
 struct guidance_indi_hybrid_params gih_params = {
   .pos_gain = GUIDANCE_INDI_POS_GAIN,
   .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
@@ -82,13 +118,15 @@ struct guidance_indi_hybrid_params gih_params = {
   .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared
   .liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
   .liftd_p50 = GUIDANCE_INDI_LIFTD_P50,
+  .min_airspeed = GUIDANCE_INDI_MIN_AIRSPEED,
+  .max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED,
+  .stall_protect_gain = 1.5, // m/s^2 downward acceleration per m/s airspeed loss
+  .climb_vspeed_fwd = GUIDANCE_INDI_FWD_CLIMB_SPEED,
+  .descend_vspeed_fwd = GUIDANCE_INDI_FWD_DESCEND_SPEED,
+  .climb_vspeed_quad = GUIDANCE_INDI_QUAD_CLIMB_SPEED,
+  .descend_vspeed_quad = GUIDANCE_INDI_QUAD_DESCEND_SPEED,
 };
 
-#ifndef GUIDANCE_INDI_MAX_AIRSPEED
-#error "You must have an airspeed sensor to use this guidance"
-#endif
-float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
-
 // Quadplanes can hover at various pref pitch
 float guidance_indi_pitch_pref_deg = 0;
 
@@ -113,7 +151,7 @@ float guidance_indi_pitch_pref_deg = 0;
 
 /*Airspeed threshold where making a turn is "worth it"*/
 #ifndef TURN_AIRSPEED_TH
-#define TURN_AIRSPEED_TH 10.0
+#define TURN_AIRSPEED_TH 13 .0
 #endif
 
 /*Boolean to force the heading to a static value (only use for specific experiments)*/
@@ -158,20 +196,21 @@ static void guidance_indi_filter_thrust(void);
 #define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5
 #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
-
 #ifndef GUIDANCE_INDI_MAX_LAT_ACCEL
 #define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81
 #endif
 
-float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD;
-float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD;
+#ifndef GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED
+#define GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED 10.0
+#endif
+
+#ifndef GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED
+#define GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED 30.0
+#endif
+
+#ifndef GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN
+#define GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN 0.0
+#endif
 
 float inv_eff[4];
 
@@ -179,6 +218,16 @@ float inv_eff[4];
 float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK;
 float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
 
+#if defined(ROTWING_STATE_FW_MAX_AIRSPEED) && defined(ROTWING_STATE_QUAD_MAX_AIRSPEED)
+  float gih_coordinated_turn_min_airspeed = ROTWING_STATE_QUAD_MAX_AIRSPEED;
+  float gih_coordinated_turn_max_airspeed = ROTWING_STATE_FW_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN;
+#else
+  float gih_coordinated_turn_min_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED;
+  float gih_coordinated_turn_max_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN;
+#endif
+
+bool coordinated_turn_use_accel = false;
+
 /** state eulers in zxy order */
 struct FloatEulers eulers_zxy;
 
@@ -192,6 +241,7 @@ Butterworth2LowPass accely_filt;
 Butterworth2LowPass guidance_indi_airspeed_filt;
 
 struct FloatVect2 desired_airspeed;
+float gi_unbounded_airspeed_sp = 0.f;
 
 float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
 struct FloatVect3 euler_cmd;
@@ -247,6 +297,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
 float time_of_vel_sp = 0.0;
 
 void guidance_indi_propagate_filters(void);
+void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -344,7 +395,7 @@ void guidance_indi_enter(void)
 
   thrust_in = stabilization.cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
-  guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;
+  guidance_indi_hybrid_heading_sp = eulers_zxy.psi;
 
   float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
@@ -352,9 +403,6 @@ void guidance_indi_enter(void)
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
   }
 
-  /*Obtain eulers with zxy rotation order*/
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
-
   init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi);
   init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
@@ -364,6 +412,11 @@ void guidance_indi_enter(void)
   init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
 }
 
+void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
+  gih_params.min_airspeed = min_airspeed;
+  gih_params.max_airspeed = max_airspeed;
+}
+
 /**
  * @param accel_sp accel setpoint in NED frame [m/s^2]
  * @param heading_sp the desired heading [rad]
@@ -459,7 +512,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   float airspeed_turn = stateGetAirspeed_f();
 #endif
   // We are dividing by the airspeed, so a lower bound is important
-  Bound(airspeed_turn, 10.0f, 30.0f);
+  Bound(airspeed_turn, gih_coordinated_turn_min_airspeed, gih_coordinated_turn_max_airspeed);
 
   guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
   guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
@@ -586,6 +639,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
   float airspeed = 0.f;
 #else
   float airspeed = stateGetAirspeed_f();
+  Bound(airspeed, 0.0f, 100.0f);
   if (guidance_indi_airspeed_filtering) {
     airspeed = guidance_indi_airspeed_filt.o[0];
   }
@@ -598,18 +652,27 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
   VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
   float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
 
-  // Make turn instead of straight line
+  gi_unbounded_airspeed_sp = norm_des_as;
+
+  // Check if some minimum airspeed is desired (e.g. to prevent stall)
+  if (norm_des_as < gih_params.min_airspeed) {
+     norm_des_as = gih_params.min_airspeed;
+  }
+
+  float gi_airspeed_sp = norm_des_as;
+
+  // Make turn instead of straight line, control airspeed
   if ((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0f))) {
 
     // Give the wind cancellation priority.
-    if (norm_des_as > guidance_indi_max_airspeed) {
+    if (norm_des_as > gih_params.max_airspeed) {
       float groundspeed_factor = 0.0f;
 
       // if the wind is faster than we can fly, just fly in the wind direction
-      if (FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) {
+      if (FLOAT_VECT2_NORM(windspeed) < gih_params.max_airspeed) {
         float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
         float bv = -2.f * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
-        float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
+        float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - gih_params.max_airspeed * gih_params.max_airspeed;
 
         float dv = bv * bv - 4.0f * av * cv;
 
@@ -625,14 +688,11 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
       desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
       desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
 
-      speed_sp_b_x = guidance_indi_max_airspeed;
+      gi_airspeed_sp = gih_params.max_airspeed;
     }
 
-    // desired airspeed can not be larger than max airspeed
-    speed_sp_b_x = Min(norm_des_as, guidance_indi_max_airspeed);
-
     if (force_forward) {
-      speed_sp_b_x = guidance_indi_max_airspeed;
+      gi_airspeed_sp = gih_params.max_airspeed;
     }
 
     // Calculate accel sp in body axes, because we need to regulate airspeed
@@ -645,7 +705,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
     BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL);
 
     // Control the airspeed
-    sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
+    sp_accel_b.x = (gi_airspeed_sp - airspeed) * gih_params.speed_gain;
 
     accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
     accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
@@ -659,8 +719,8 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
       float speed_increment = speed_sp_b_x - groundspeed_x;
 
       // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
-      if ((speed_increment + airspeed) > guidance_indi_max_airspeed) {
-        speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
+      if ((speed_increment + airspeed) > gih_params.max_airspeed) {
+        speed_sp_b_x = gih_params.max_airspeed + groundspeed_x - airspeed;
       }
     }
 
@@ -673,22 +733,25 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
   }
 
   // Bound the acceleration setpoint
-  float accelbound = 3.0f + airspeed / guidance_indi_max_airspeed * 5.0f; // FIXME remove hard coded values
+  float accelbound = 3.0f + airspeed / gih_params.max_airspeed * 5.0f; // FIXME remove hard coded values
   float_vect3_bound_in_2d(&accel_sp, accelbound);
-  /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
-  /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
   BoundAbs(accel_sp.z, 3.0);
 
+  if (!rotwing_state_pusher_motor_running() && !rotwing_state_hover_motors_running()) {
+    accel_sp.z = gih_params.stall_protect_gain * (gi_airspeed_sp - airspeed);
+    BoundAbs(accel_sp.z, 5.0);
+  }
+
   return accel_sp;
 }
 
 static float bound_vz_sp(float vz_sp)
 {
   // Bound vertical speed setpoint
-  if (stateGetAirspeed_f() > 13.f) {
-    Bound(vz_sp, -climb_vspeed_fwd, -descend_vspeed_fwd);
+  if (stateGetAirspeed_f() > TURN_AIRSPEED_TH) {
+    Bound(vz_sp, -gih_params.climb_vspeed_fwd, -gih_params.descend_vspeed_fwd);
   } else {
-    Bound(vz_sp, -nav.climb_vspeed, -nav.descend_vspeed); // FIXME don't use nav settings
+    Bound(vz_sp, -gih_params.climb_vspeed_quad, -gih_params.descend_vspeed_quad);
   }
   return vz_sp;
 }
@@ -804,6 +867,7 @@ void guidance_indi_propagate_filters(void)
   update_butterworth_2_low_pass(&accely_filt, accely);
 
   float airspeed = stateGetAirspeed_f();
+  Bound(airspeed, 0.0f, 100.0f);
   update_butterworth_2_low_pass(&guidance_indi_airspeed_filt, airspeed);
 }
 

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

@@ -70,6 +70,7 @@ enum GuidanceIndiHybrid_VMode {
 
 extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
 extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
+extern void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed);
 
 struct guidance_indi_hybrid_params {
   float pos_gain;
@@ -80,30 +81,32 @@ struct guidance_indi_hybrid_params {
   float liftd_asq;
   float liftd_p80;
   float liftd_p50;
+  float min_airspeed;
+  float max_airspeed;
+  float stall_protect_gain;
+  float climb_vspeed_fwd;
+  float descend_vspeed_fwd;
+  float climb_vspeed_quad;
+  float descend_vspeed_quad;
 };
 
 extern struct FloatVect3 sp_accel;
 extern struct FloatVect3 gi_speed_sp;
+extern struct guidance_indi_hybrid_params gih_params;
 
 extern float guidance_indi_pitch_pref_deg;
 
-#if GUIDANCE_INDI_HYBRID_USE_WLS
-extern float Wu_gih[GUIDANCE_INDI_HYBRID_U];
-extern float Wv_gih[GUIDANCE_INDI_HYBRID_V];
-extern float du_min_gih[GUIDANCE_INDI_HYBRID_U];
-extern float du_max_gih[GUIDANCE_INDI_HYBRID_U];
-extern float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
-#endif
+extern float gi_unbounded_airspeed_sp;
 
 extern float guidance_indi_thrust_z_eff;
 
 extern struct guidance_indi_hybrid_params gih_params;
 extern float guidance_indi_specific_force_gain;
-extern float guidance_indi_max_airspeed;
 extern bool take_heading_control;
 extern float guidance_indi_max_bank;
 extern float guidance_indi_min_pitch;
 extern bool force_forward;       ///< forward flight for hybrid nav
 extern bool guidance_indi_airspeed_filtering;
+extern bool coordinated_turn_use_accel;
 
 #endif /* GUIDANCE_INDI_HYBRID_H */

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -267,9 +267,12 @@ void nav_periodic_task(void)
 
 bool nav_detect_ground(void)
 {
-  if (!autopilot.ground_detected) { return false; }
-  autopilot.ground_detected = false;
-  return true;
+  if (autopilot.ground_detected) {
+    autopilot.ground_detected = false;
+    return true;
+  } else {
+    return false;
+  }
 }
 
 bool nav_is_in_flight(void)

sw/airborne/modules/ctrl/eff_scheduling_rotwing.c

@@ -130,6 +130,7 @@ struct rotwing_eff_sched_param_t eff_sched_p = {
   .Ixx_wing                 = ROTWING_EFF_SCHED_IXX_WING,
   .Iyy_wing                 = ROTWING_EFF_SCHED_IYY_WING,
   .m                        = ROTWING_EFF_SCHED_M,
+  .DMdpprz_hover_pitch      = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
   .DMdpprz_hover_roll       = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
   .hover_roll_pitch_coef    = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
   .hover_roll_roll_coef     = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
@@ -146,9 +147,28 @@ struct rotwing_eff_sched_param_t eff_sched_p = {
   .k_lift_tail              = ROTWING_EFF_SCHED_K_LIFT_TAIL
 };
 
-float eff_sched_pusher_time = 0.002;
+int32_t rw_flap_offset = 0;
 
-float roll_eff_scaling = 1.f;
+// for negative values, still should be low_lim < up_lim
+inline float bound_or_zero(float value, float low_lim, float up_lim) {
+  float output = value;
+  if (low_lim > 0.f) {
+    if (value < low_lim) {
+      output = 0.f;
+    } else if (value > up_lim) {
+      output = up_lim;
+    }
+  } else {
+    if (value > up_lim) {
+      output = 0.f;
+    } else if (value < low_lim) {
+      output = low_lim;
+    }
+  }
+  return output;
+}
+
+float eff_sched_pusher_time = 0.002;
 
 struct rotwing_eff_sched_var_t eff_sched_var;
 
@@ -204,8 +224,9 @@ void eff_scheduling_rotwing_init(void)
   eff_sched_var.sinr3             = 0;
 
   // Set moment derivative variables
-  eff_sched_var.pitch_motor_dMdpprz = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH;
-  eff_sched_var.roll_motor_dMdpprz  = (eff_sched_p.DMdpprz_hover_roll[0] + (MAX_PPRZ/2.) * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
+  float hover_thrust = 6000;
+  eff_sched_var.pitch_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.; // Dmdpprz hover pitch for hover thrust
+  eff_sched_var.roll_motor_dMdpprz  = (eff_sched_p.DMdpprz_hover_roll[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
 
   eff_sched_var.cmd_elevator = 0;
   eff_sched_var.cmd_pusher = 0;
@@ -282,44 +303,45 @@ void eff_scheduling_rotwing_update_airspeed(void)
 
 void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
 {
-  // Pitch motor effectiveness
+  float cmd_quat[4];
+  float dM_dpprz[4];
+  // Quadratic thrust (and therefore moment) model of the hover propellers
+  for (uint8_t i = 0; i < 4; i++) {
+    cmd_quat[i] = actuator_state_filt_vect[i];
+    Bound(cmd_quat[i], 2500, MAX_PPRZ);
 
-  float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;
-
-  float cmd_right = actuator_state_filt_vect[1];
-  float cmd_left = actuator_state_filt_vect[3];
-  Bound(cmd_right, 3500, MAX_PPRZ);
-  Bound(cmd_left, 3500, MAX_PPRZ);
+    if(i==0 || i==2) { // pitch motors
+      dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.;
+      // Bound dM_dpprz to half and 2 times the hover effectiveness
+      Bound(dM_dpprz[i], eff_sched_var.pitch_motor_dMdpprz * 0.5, eff_sched_var.pitch_motor_dMdpprz * 2.0);
+    } else { // roll motors
+      dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_roll[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
+      Bound(dM_dpprz[i], eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
+    }
+  }
 
   // Roll motor effectiveness
-  float dM_dpprz_right  = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_right * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
-  float dM_dpprz_left   = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_left  * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
-
-  dM_dpprz_right = dM_dpprz_right * roll_eff_scaling;
-  dM_dpprz_left = dM_dpprz_left * roll_eff_scaling;
-
-  // Bound dM_dpprz to half and 2 times the hover effectiveness
-  Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
-  Bound(dM_dpprz_left,  eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
+  float dM_dpprz_right  = dM_dpprz[1];
+  float dM_dpprz_left   = dM_dpprz[3];;
 
   float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
-  Bound(roll_motor_p_eff_right, -1, -0.00001);
+  roll_motor_p_eff_right = bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f);
 
   float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
   if(autopilot.in_flight) {
     float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx;
     roll_motor_p_eff_left += roll_motor_airspeed_compensation;
   }
-  Bound(roll_motor_p_eff_left, 0.00001, 1);
+  roll_motor_p_eff_left = bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f);
 
   float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
   Bound(roll_motor_q_eff, 0, 1);
 
   // Update front pitch motor q effectiveness
-  g1g2[1][0] = pitch_motor_q_eff;   // pitch effectiveness front motor
+  g1g2[1][0] = dM_dpprz[0] / eff_sched_var.Iyy;   // pitch effectiveness front motor
 
   // Update back motor q effectiveness
-  g1g2[1][2] = -pitch_motor_q_eff;  // pitch effectiveness back motor
+  g1g2[1][2] = - dM_dpprz[2] / eff_sched_var.Iyy;  // pitch effectiveness back motor
 
   // Update right motor p and q effectiveness
   g1g2[0][1] = roll_motor_p_eff_right;   // roll effectiveness right motor (no airspeed compensation)
@@ -376,6 +398,11 @@ void eff_scheduling_rotwing_update_aileron_effectiveness(void)
   float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx;
   Bound(eff_x_aileron, 0, 0.005)
   g1g2[0][6] = eff_x_aileron;
+
+  float dMydpprz = 4.0*(eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr2 * eff_sched_var.cosr) / 1000000.;
+  float eff_y_aileron = dMydpprz / eff_sched_var.Iyy;
+  eff_y_aileron = bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
+  g1g2[1][6] = eff_y_aileron;
 }
 
 void eff_scheduling_rotwing_update_flaperon_effectiveness(void)
@@ -427,11 +454,18 @@ void stabilization_indi_set_wls_settings(void)
       wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i];
       wls_stab_p.u_max[i] = MAX_PPRZ;
       wls_stab_p.u_pref[i] = act_pref[i];
-      if (i == 5) {
-        wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in preferred state to 0 for elevator
+      if (i == 5) { // elevator
+        wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
         wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator
       }
-      if (i==8) {
+      if (i == 7) { // flaperons
+        // If an offset is used, limit the max differential command to prevent unilateral saturation.
+        int32_t flap_saturation_limit = MAX_PPRZ - abs(rw_flap_offset);
+        BoundAbs(flap_saturation_limit, MAX_PPRZ);
+        wls_stab_p.u_min[i] = -flap_saturation_limit;
+        wls_stab_p.u_max[i] = flap_saturation_limit;
+      }
+      if (i==8) { // pusher
         // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002
         Bound(eff_sched_pusher_time, 0.002, 5.);
         float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500);

sw/airborne/modules/ctrl/eff_scheduling_rotwing.h

@@ -36,6 +36,7 @@ struct rotwing_eff_sched_param_t {
   float Iyy_wing;                 // wing MMOI around the spanwise direction of the wing [kgm²]
   float m;                        // mass [kg]
   float DMdpprz_hover_roll[2];    // Moment coeficients for roll motors (Scaled by 10000)
+  float DMdpprz_hover_pitch[2];   // Moment coeficients for pitch motors (Scaled by 10000)
   float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors
   float hover_roll_roll_coef[2];  // Model coefficients to correct roll effectiveness for roll motors
   float k_elevator[3];
@@ -50,6 +51,7 @@ struct rotwing_eff_sched_param_t {
   float k_lift_fuselage;
   float k_lift_tail;
 };
+extern struct rotwing_eff_sched_param_t eff_sched_p;
 
 struct rotwing_eff_sched_var_t {
   float Ixx;                  // Total MMOI around roll axis [kgm²]
@@ -77,12 +79,13 @@ struct rotwing_eff_sched_var_t {
   float airspeed2;
 };
 
-extern float roll_eff_scaling;
+extern int32_t rw_flap_offset;
 
 extern float rotation_angle_setpoint_deg;
 extern int16_t rotation_cmd;
 
 extern float eff_sched_pusher_time;
+extern float roll_eff_slider;
 
 extern void eff_scheduling_rotwing_init(void);
 extern void eff_scheduling_rotwing_periodic(void);

sw/airborne/modules/nav/nav_rotorcraft_hybrid.c

@@ -87,12 +87,18 @@ float nav_hybrid_line_gain = 1.0f;
 #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f
 #endif
 
-#ifdef NAV_HYBRID_POS_GAIN
+#ifdef GUIDANCE_INDI_POS_GAIN
+float nav_hybrid_pos_gain = GUIDANCE_INDI_POS_GAIN;
+#elif defined(NAV_HYBRID_POS_GAIN)
 float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN;
 #else
 float nav_hybrid_pos_gain = 1.0f; 
 #endif
 
+#if defined(NAV_HYBRID_POS_GAIN) && defined(GUIDANCE_INDI_POS_GAIN)
+#warning "NAV_HYBRID_POS_GAIN and GUIDANCE_INDI_POS_GAIN serve similar purpose and are both defined, using GUIDANCE_INDI_POS_GAIN"
+#endif
+
 #ifndef GUIDANCE_INDI_HYBRID
 bool force_forward = 0.0f;
 #endif

sw/airborne/modules/rotwing_drone/rotwing_state.h

@@ -28,70 +28,66 @@
 
 #include "std.h"
 
-/** Rotwing States
- */
-#define ROTWING_STATE_HOVER               0 // Wing is skewed in 0 degrees (quad)
-#define ROTWING_STATE_SKEWING             1 // WIng is skewing
-#define ROTWING_STATE_FW                  2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority
-#define ROTWING_STATE_FW_HOV_MOT_IDLE     3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle
-#define ROTWING_STATE_FW_HOV_MOT_OFF      4 // Wing is skewed at 90 degrees (fixed wubg), hover motors are switched off
-#define ROTWING_STATE_FREE                5 // This is a desired state for which the controller has to decide the desired state itself
-
-/** Rotwing Configurations
- */
-#define ROTWING_CONFIGURATION_HOVER       0 // UAV is in hover
-#define ROTWING_CONFIGURATION_HYBRID      1 // UAV can fly forward and hover if airspeed low, hover motors on
-#define ROTWING_CONFIGURATION_EFFICIENT   2 // UAV flies efficiently forward  (FW)
-#define ROTWING_CONFIGURATION_FREE        3 // UAV switched between efficient and hybrid dependent on deceleration 
-
-struct RotwingState {
-  uint8_t current_state;
-  uint8_t desired_state;
-  uint8_t requested_config;
+enum rotwing_states_t {
+  ROTWING_STATE_FORCE_HOVER,
+  ROTWING_STATE_REQUEST_HOVER,
+  ROTWING_STATE_FORCE_FW,
+  ROTWING_STATE_REQUEST_FW,
+  ROTWING_STATE_FREE,
 };
 
-#define ROTWING_STATE_WING_QUAD_SETTING         0 // Wing skew at 0 degrees
-#define ROTWING_STATE_WING_SCHEDULING_SETTING   1 // Wing skew handled by airspeed scheduler
-#define ROTWING_STATE_WING_FW_SETTING           2 // Wing skew at 90 degrees
-
-#define ROTWING_STATE_PITCH_QUAD_SETTING        0 // Pitch at prefered hover
-#define ROTWING_STATE_PITCH_TRANSITION_SETTING  1 // Pitch scheduled
-#define ROTWING_STATE_PITCH_FW_SETTING          2 // Pitch at prefered forward
-
-struct RotWingStateSettings {
-  uint8_t wing_scheduler;
-  bool hover_motors_active;
-  bool hover_motors_disable;
-  bool force_forward;
-  uint8_t preferred_pitch;
-  bool stall_protection;
-  float max_v_climb;
-  float max_v_descend;
-  float nav_max_speed;
+union rotwing_bitmask_t {
+  uint16_t value;
+  struct {
+    bool skew_angle_valid : 1;      // Skew angle didn't timeout
+    bool hover_motors_enabled : 1;  // Hover motors command is enabled
+    bool hover_motors_idle : 1;     // Hover motors are idling (throttle < IDLE_THROTTLE)
+    bool hover_motors_running : 1;  // Hover motors are running (RPM >= MIN_RPM)
+    bool pusher_motor_running : 1;  // Pusher motor is running (RPM >= MIN_RPM)
+    bool skew_forced : 1;           // Skew angle is forced
+  };
 };
 
-struct RotWingStateSkewing {
-  float wing_angle_deg_sp;   // Wing angle setpoint in deg
-  float wing_angle_deg;      // Wing angle from sensor in deg
-  int32_t servo_pprz_cmd;    // Wing rotation servo pprz cmd
-  bool airspeed_scheduling;  // Airspeed scheduling on or off
-  bool force_rotation_angle; // Setting to force wing_angle_deg_sp
+struct rotwing_state_t {
+  /* Control */
+  enum rotwing_states_t state;      // Current state
+  enum rotwing_states_t nav_state;  // Desired navigation state (requested only by NAV and can be overruled by RC)
+  bool hover_motors_enabled;        // Hover motors enabled (> idle throttle)
+
+  /* Skew */
+  float sp_skew_angle_deg;        // Setpoint skew angle in degrees
+  float ref_model_skew_angle_deg; // Reference model skew angle in degrees
+  float meas_skew_angle_deg;      // Measured skew angle in degrees
+  float meas_skew_angle_time;     // Time of the last skew angle measurement
+  bool force_skew;                // Force skew angle to a certain value by the GCS
+  int16_t skew_cmd;               // Skewing command in pprz values
+
+  /* Airspeeds */
+  float fw_min_airspeed;      // Minimum airspeed (stall+margin)
+  float cruise_airspeed;      // Airspeed for cruising
+  float min_airspeed;         // Minimum airspeed for bounding
+  float max_airspeed;         // Maximum airspeed for bounding
+
+  /* RPM measurements*/
+  int32_t meas_rpm[5];        // Measured RPM of the hover and pusher motors
+  float meas_rpm_time[5];     // Time of the last RPM measurement
+
+  /* Sim failures */
+  bool fail_skew_angle;       // Skew angle sensor failure
+  bool fail_hover_motor;      // Hover motor failure
+  bool fail_pusher_motor;     // Pusher motor failure
 };
+extern struct rotwing_state_t rotwing_state;
 
-extern struct RotwingState rotwing_state;
-extern struct RotWingStateSettings rotwing_state_settings;
-extern struct RotWingStateSkewing rotwing_state_skewing;
+void rotwing_state_init(void);
+void rotwing_state_periodic(void);
+bool rotwing_state_hover_motors_running(void);
+bool rotwing_state_pusher_motor_running(void);
+bool rotwing_state_skew_angle_valid(void);
 
-extern float rotwing_state_max_hover_speed;
-
-extern bool hover_motors_active;
-extern bool bool_disable_hover_motors;
-
-extern void init_rotwing_state(void);
-extern void periodic_rotwing_state(void);
-extern void request_rotwing_state(uint8_t state);
-extern void rotwing_request_configuration(uint8_t configuration);
-extern void rotwing_state_skew_actuator_periodic(void);
-extern bool rotwing_state_hover_motors_running(void);
+void rotwing_state_set(enum rotwing_states_t state);
+bool rotwing_state_choose_circle_direction(uint8_t wp_id);
+void rotwing_state_set_transition_wp(uint8_t wp_id);
+void rotwing_state_update_WP_height(uint8_t wp_id, float height);
 
 #endif  // ROTWING_STATE_H

sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py

@@ -70,14 +70,14 @@ class AHRSRefQuatMessage(object):
     
 class RotWingControllerMessage(object):
     def __init__(self, msg):
-        self.wing_angle_deg = msg['wing_angle_deg']
-        self.wing_angle_deg_sp = msg['wing_angle_deg_sp']
+        self.meas_skew_angle_deg = msg['meas_skew_angle']
+        self.sp_skew_angle_deg = msg['sp_skew_angle']
 
     def get_wing_angle(self):
-        return np.deg2rad(float(self.wing_angle_deg))
+        return np.deg2rad(float(self.meas_skew_angle_deg))
     
     def get_wing_angle_sp(self):
-        return np.deg2rad(float(self.wing_angle_deg_sp))
+        return np.deg2rad(float(self.sp_skew_angle_deg))
     
 class ActuatorsMessage(object):
     def __init__(self, msg):