Sync guidance indi hybrid and guidance indi hybrid quadplane/tailsitter filters (#3541)

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

@@ -236,6 +236,7 @@ float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
 Butterworth2LowPass roll_filt;
 Butterworth2LowPass pitch_filt;
+Butterworth2LowPass yaw_filt;
 Butterworth2LowPass thrust_filt;
 Butterworth2LowPass accely_filt;
 Butterworth2LowPass guidance_indi_airspeed_filt;
@@ -360,6 +361,7 @@ void guidance_indi_init(void)
   }
   init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
+  init_butterworth_2_low_pass(&yaw_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 
@@ -404,6 +406,7 @@ void guidance_indi_enter(void)
 
   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(&yaw_filt, tau, sample_time, eulers_zxy.psi);
   init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
   init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
 

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

@@ -111,4 +111,8 @@ extern bool force_forward;       ///< forward flight for hybrid nav
 extern bool guidance_indi_airspeed_filtering;
 extern bool coordinated_turn_use_accel;
 
+extern Butterworth2LowPass roll_filt;
+extern Butterworth2LowPass pitch_filt;
+extern Butterworth2LowPass yaw_filt;
+
 #endif /* GUIDANCE_INDI_HYBRID_H */

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

@@ -44,7 +44,13 @@
 float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
 #endif
 
-float bodyz_filter_cutoff = 0.2;
+#ifndef GUIDANCE_INDI_BODYZ_FILTER_CUTOFF
+#ifdef GUIDANCE_INDI_FILTER_CUTOFF
+#define GUIDANCE_INDI_BODYZ_FILTER_CUTOFF GUIDANCE_INDI_FILTER_CUTOFF
+#else
+#define GUIDANCE_INDI_BODYZ_FILTER_CUTOFF 3.0
+#endif
+#endif
 
 Butterworth2LowPass accel_bodyz_filt;
 
@@ -53,7 +59,7 @@ Butterworth2LowPass accel_bodyz_filt;
  * Call upon entering indi guidance
  */
 void guidance_indi_quadplane_init(void) {
-  float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff);
+  float tau_bodyz = 1.0/(2.0*M_PI*GUIDANCE_INDI_BODYZ_FILTER_CUTOFF);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
   init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
 }
@@ -78,17 +84,13 @@ void guidance_indi_quadplane_propagate_filters(void) {
  * @param body_v 3D vector to write the control objective v
  */
 void 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]) {
-  // Get attitude
-  struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
-
   /*Pre-calculate sines and cosines*/
-  float sphi = sinf(eulers_zxy.phi);
+  float sphi = sinf(roll_filt.o[0]);
-  float cphi = cosf(eulers_zxy.phi);
+  float cphi = cosf(roll_filt.o[0]);
-  float stheta = sinf(eulers_zxy.theta);
+  float stheta = sinf(pitch_filt.o[0]);
-  float ctheta = cosf(eulers_zxy.theta);
+  float ctheta = cosf(pitch_filt.o[0]);
-  float spsi = sinf(eulers_zxy.psi);
+  float spsi = sinf(yaw_filt.o[0]);
-  float cpsi = cosf(eulers_zxy.psi);
+  float cpsi = cosf(yaw_filt.o[0]);
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0

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

@@ -39,18 +39,14 @@
  */
 
 void 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]) {
-  // Get attitude
-  struct FloatEulers eulers_zxy;
-  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
-
 
   /*Pre-calculate sines and cosines*/
-  float sphi = sinf(eulers_zxy.phi);
+  float sphi = sinf(roll_filt.o[0]);
-  float cphi = cosf(eulers_zxy.phi);
+  float cphi = cosf(roll_filt.o[0]);
-  float stheta = sinf(eulers_zxy.theta);
+  float stheta = sinf(pitch_filt.o[0]);
-  float ctheta = cosf(eulers_zxy.theta);
+  float ctheta = cosf(pitch_filt.o[0]);
-  float spsi = sinf(eulers_zxy.psi);
+  float spsi = sinf(yaw_filt.o[0]);
-  float cpsi = cosf(eulers_zxy.psi);
+  float cpsi = cosf(yaw_filt.o[0]);
   //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
 
 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
@@ -58,13 +54,13 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
 #endif
 
   /*Amount of lift produced by the wing*/
-  float pitch_lift = eulers_zxy.theta;
+  float pitch_lift = pitch_filt.o[0];
   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);
+  float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), pitch_filt.o[0]);
 
   Gmat[0][0] =  cphi*ctheta*spsi*T + cphi*spsi*lift;
   Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;