hybrid quadplanes and tailsitters explicit

conf/airframes/examples/cube_orange.xml

@@ -109,7 +109,7 @@
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
     </module-->
 
-    <module name="guidance" type="indi_hybrid">
+    <module name="guidance" type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>

conf/airframes/tudelft/bebop2_indi_convergence.xml

@@ -34,7 +34,7 @@
     <module name="gps" type="ublox"/>
     <module name="gps" type="ubx_ucenter"/>
     <module name="stabilization" type="indi_simple"/>
-    <module name="guidance" type="indi_hybrid"/>
+    <module name="guidance" type="indi_hybrid_tailsitter"/>
 
     <module name="ins" type="ekf2"/>
     <!--<module name="ahrs" type="int_cmpl_quat">-->

conf/airframes/tudelft/cyfoam.xml

@@ -292,7 +292,7 @@
       <define name="WLS_N_U" value="4" />
       <define name="WLS_N_V" value="4" />
     </module>
-    <module name="guidance"      type="indi_hybrid">
+    <module name="guidance"      type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>

conf/airframes/tudelft/disco_rotorcraft_indi.xml

@@ -60,7 +60,7 @@
       <define name="WLS_N_V" value="4"/>
      <configure name="INDI_NUM_ACT" value="3"/>
     </module>
-    <module name="guidance" type="indi_hybrid"/>
+    <module name="guidance" type="indi_hybrid_tailsitter"/>
     <!--<module name="sonar_bebop">
       <define name="USE_SONAR"/>
       <define name="SENSOR_SYNC_SEND_SONAR"/>

conf/airframes/tudelft/nederdrone4.xml

@@ -150,7 +150,7 @@
       <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
     </module>
 
-    <module name="guidance" type="indi_hybrid">
+    <module name="guidance" type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>

conf/airframes/tudelft/nederdrone6.xml

@@ -138,7 +138,7 @@
       <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
     </module>
 
-    <module name="guidance" type="indi_hybrid">
+    <module name="guidance" type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>

conf/airframes/tudelft/nederdrone7.xml

@@ -138,7 +138,7 @@
       <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
     </module>
 
-    <module name="guidance" type="indi_hybrid">
+    <module name="guidance" type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>

conf/airframes/tudelft/nederdrone8.xml

@@ -138,7 +138,7 @@
       <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
     </module>
 
-    <module name="guidance" type="indi_hybrid">
+    <module name="guidance" type="indi_hybrid_tailsitter">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>

conf/airframes/tudelft/rot_wing_25kg.xml

@@ -98,7 +98,7 @@
 
         <module name="ctrl_eff_sched_rot_wing"/>
 
-        <module name="guidance" type="indi_hybrid"/>
+        <module name="guidance" type="indi_hybrid_quadplane"/>
         <module name="nav_hybrid"/>
 
         <!-- Other -->
@@ -402,7 +402,6 @@
         <define name="PITCH_LIFT_EFF"       value="0.0"/>
         <define name="ZERO_AIRSPEED"        value="TRUE"/>
 
-        <define name="QUADPLANE"            value="TRUE"/>
         <define name="THRUST_Z_EFF"         value="-0.0023"/>
         <define name="THRUST_X_EFF"         value="0.00055"/>
 

conf/modules/guidance_indi_hybrid.xml

@@ -38,7 +38,6 @@
   </header>
   <init fun="guidance_indi_init()"/>
   <init fun="guidance_indi_enter()"/>
-  <!--<periodic fun="guidance_indi_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>-->
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="guidance_indi_hybrid.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <define name="GUIDANCE_INDI_HYBRID" value="TRUE"/>

conf/modules/guidance_indi_hybrid_quadplane.xml

@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="guidance_indi_hybrid_quadplane" dir="guidance" task="control">
+  <doc>
+    <description>
+      Guidance controller for hybrid quadplane vehicles
+    </description>
+  </doc>
+  <dep>
+    <depends>guidance_indi_hybrid</depends>
+  </dep>
+  <header>
+    <file name="guidance_indi_hybrid_quadplane.h"/>
+  </header>
+  <init fun="guidance_indi_quadplane_init()"/>
+  <periodic fun="guidance_indi_quadplane_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>
+  <makefile target="ap|nps" firmware="rotorcraft">
+    <!-- Guidance actuators: (roll, pitch, thrust, push) for quadplanes  -->
+    <define name="GUIDANCE_INDI_HYBRID_U" value="4"/>
+    <!-- Guidance control objectives: ax, ay, ay for quadplanes -->
+    <define name="GUIDANCE_INDI_HYBRID_V" value="3"/>
+    <file name="guidance_indi_hybrid_quadplane.c" dir="$(SRC_FIRMWARE)/guidance"/>
+  </makefile>
+</module>

conf/modules/guidance_indi_hybrid_tailsitter.xml

@@ -0,0 +1,22 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="guidance_indi_hybrid_tailsitter" dir="guidance" task="control">
+  <doc>
+    <description>
+      Guidance controller for hybrid tailsitter vehicles
+    </description>
+  </doc>
+  <dep>
+    <depends>guidance_indi_hybrid</depends>
+  </dep>
+  <header>
+    <file name="guidance_indi_hybrid_tailsitter.h"/>
+  </header>
+  <makefile target="ap|nps" firmware="rotorcraft">
+    <!-- Guidance actuators: (roll, pitch, thrust, push) for quadplanes  -->
+    <define name="GUIDANCE_INDI_HYBRID_U" value="3"/>
+    <!-- Guidance control objectives: ax, ay, ay for quadplanes -->
+    <define name="GUIDANCE_INDI_HYBRID_V" value="3"/>
+    <file name="guidance_indi_hybrid_tailsitter.c" dir="$(SRC_FIRMWARE)/guidance"/>
+  </makefile>
+</module>

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

@@ -90,20 +90,6 @@ float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
 float guidance_indi_pitch_pref_deg = 0;
 
 
-#if GUIDANCE_INDI_QUADPLANE
-#ifndef GUIDANCE_INDI_THRUST_Z_EFF
-#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF"
-#else
-float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
-#endif
-
-#ifndef GUIDANCE_INDI_THRUST_X_EFF
-#error "You need to define GUIDANCE_INDI_THRUST_X_EFF"
-#else
-float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
-#endif
-#endif
-
 // If using WLS, check that the matrix size is sufficient
 #if GUIDANCE_INDI_HYBRID_USE_WLS
 #if GUIDANCE_INDI_HYBRID_U > WLS_N_U
@@ -181,7 +167,6 @@ Butterworth2LowPass roll_filt;
 Butterworth2LowPass pitch_filt;
 Butterworth2LowPass thrust_filt;
 Butterworth2LowPass accely_filt;
-Butterworth2LowPass accel_bodyz_filt;
 
 struct FloatVect2 desired_airspeed;
 
@@ -207,7 +192,6 @@ float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f };
 #endif
 
 float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
-float bodyz_filter_cutoff = 0.2;
 
 float guidance_indi_hybrid_heading_sp = 0.f;
 struct FloatEulers guidance_euler_cmd;
@@ -224,7 +208,6 @@ 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_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]);
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -289,7 +272,6 @@ void guidance_indi_enter(void) {
   guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;
 
   float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
-  float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
   for (int8_t i = 0; i < 3; i++) {
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
@@ -302,7 +284,6 @@ void guidance_indi_enter(void) {
   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);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
-  init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
 }
 
 #include "firmwares/rotorcraft/navigation.h"
@@ -735,166 +716,8 @@ void guidance_indi_propagate_filters(void) {
   // Propagate filter for sideslip correction
   float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
   update_butterworth_2_low_pass(&accely_filt, accely);
-
-  // Propagate filter for thrust/lift estimate
-  float accelz = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->z);
-  update_butterworth_2_low_pass(&accel_bodyz_filt, accelz);
-}
-
-/**
- * Calculate the matrix of partial derivatives of the roll, pitch and thrust
- * w.r.t. the NED accelerations, taking into account the lift of a wing that is
- * horizontal at -90 degrees pitch
- *
- * @param Gmat Dynamics matrix
- * @param a_diff acceleration errors in earth frame
- * @param body_v 3D vector to write the control objective v
- */
-#if defined(GUIDANCE_INDI_QUADPLANE)
-/**
- * Perform WLS
- *
- * @param Gmat Dynamics matrix
- * @param a_diff acceleration errors in earth frame
- * @param body_v 3D vector to write the control objective v
- */
-void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V]) {
-  /*Pre-calculate sines and cosines*/
-  float sphi = sinf(eulers_zxy.phi);
-  float cphi = cosf(eulers_zxy.phi);
-  float stheta = sinf(eulers_zxy.theta);
-  float ctheta = cosf(eulers_zxy.theta);
-  float spsi = sinf(eulers_zxy.psi);
-  float cpsi = cosf(eulers_zxy.psi);
-
-#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
-#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
-#endif
-
-  /*Amount of lift produced by the wing*/
-  float lift_thrust_bz = accel_bodyz_filt.o[0]; // Sum of lift and thrust in boxy z axis (level flight)
-
-  // get the derivative of the lift wrt to theta
-  float liftd = guidance_indi_get_liftd(0.0f, 0.0f);
-
-  Gmat[0][0] = -sphi*stheta*lift_thrust_bz;
-  Gmat[1][0] = -cphi*lift_thrust_bz;
-  Gmat[2][0] = -sphi*ctheta*lift_thrust_bz;
-
-  Gmat[0][1] =  cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
-  Gmat[1][1] =  sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd;
-  Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
-
-  Gmat[0][2] =  cphi*stheta;
-  Gmat[1][2] = -sphi;
-  Gmat[2][2] =  cphi*ctheta;
-
-  Gmat[0][3] =  ctheta;
-  Gmat[1][3] =  0;
-  Gmat[2][3] = -stheta;
-
-  // Convert acceleration error to body axis system
-  body_v[0] =  cpsi * a_diff.x + spsi * a_diff.y;
-  body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y;
-  body_v[2] =  a_diff.z;
-}
-#else
-void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) {
-
-  /*Pre-calculate sines and cosines*/
-  float sphi = sinf(eulers_zxy.phi);
-  float cphi = cosf(eulers_zxy.phi);
-  float stheta = sinf(eulers_zxy.theta);
-  float ctheta = cosf(eulers_zxy.theta);
-  float spsi = sinf(eulers_zxy.psi);
-  float cpsi = cosf(eulers_zxy.psi);
-  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
-
-#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
-#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
-#endif
-
-  /*Amount of lift produced by the wing*/
-  float pitch_lift = eulers_zxy.theta;
-  Bound(pitch_lift,-M_PI_2,0);
-  float lift = sinf(pitch_lift)*9.81;
-  float T = cosf(pitch_lift)*-9.81;
-
-  // get the derivative of the lift wrt to theta
-  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);
-
-  Gmat[0][0] =  cphi*ctheta*spsi*T + cphi*spsi*lift;
-  Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
-  Gmat[2][0] = -sphi*ctheta*T -sphi*lift;
-  Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
-  Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
-  Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
-  Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
-  Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
-  Gmat[2][2] = cphi*ctheta;
-
-  v_gih[0] = a_diff.x;
-  v_gih[1] = a_diff.y;
-  v_gih[2] = a_diff.z;
-}
-#endif
-/**
- *
- * @param body_v
- *
- * WEAK function to set the quadplane wls settings
- */
-#if GUIDANCE_INDI_HYBRID_USE_WLS
-void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle, float pitch_angle)
-{
-  // Set lower limits
-  du_min_gih[0] = -guidance_indi_max_bank - roll_angle; // roll
-  du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // pitch
-  du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3];
-
-  // Set upper limits limits
-  du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll
-  du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // pitch
-  du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]);
-
-  // Set prefered states
-  du_pref_gih[0] = -roll_angle; // prefered delta roll angle
-  du_pref_gih[1] = -pitch_angle; // prefered delta pitch angle
-  du_pref_gih[2] = du_max_gih[2];
 }
-#elif defined(GUIDANCE_INDI_QUADPLANE)
-#warning We have GUIDANCE_INDI_QUADPLANE
-void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
-{
-  float roll_limit_rad = GUIDANCE_H_MAX_BANK;
-  float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
-  float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
-
-  float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
-
-  // Set lower limits
-  du_min_gih[0] = -roll_limit_rad - roll_angle; //roll
-  du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch
-  du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
-  du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;
-
-  // Set upper limits limits
-  du_max_gih[0] = roll_limit_rad - roll_angle; //roll
-  du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch
-  du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
-  du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;
-
-  // Set prefered states
-  du_pref_gih[0] = -roll_angle; // prefered delta roll angle
-  du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
-  du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency
-  du_pref_gih[3] = body_v[0]; // solve the body acceleration
-}
-#else
-void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle UNUSED, float pitch_angle UNUSED) {
 
-}
-#endif
 
 /**
  * @brief Get the derivative of lift w.r.t. pitch.

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

@@ -37,12 +37,6 @@
 #include "firmwares/rotorcraft/guidance.h"
 #include "firmwares/rotorcraft/stabilization.h"
 
-#ifdef GUIDANCE_INDI_QUADPLANE
-#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h"
-#define GUIDANCE_INDI_HYBRID_USE_WLS true
-#else
-#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h"
-#endif
 
 
 // TODO change names for _indi_hybrid_
@@ -51,6 +45,7 @@ extern void guidance_indi_init(void);
 extern void guidance_indi_enter(void);
 extern float guidance_indi_get_liftd(float pitch, float theta);
 extern void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
+extern void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]);
 
 enum GuidanceIndiHybrid_HMode {
   GUIDANCE_INDI_HYBRID_H_POS,