diff --git a/conf/modules/guidance_indi_hybrid.xml b/conf/modules/guidance_indi_hybrid.xml
index c083ed67ad2..f15184c502e 100644
--- a/conf/modules/guidance_indi_hybrid.xml
+++ b/conf/modules/guidance_indi_hybrid.xml
@@ -28,6 +28,7 @@
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index b400dc14d61..27990b3ef3e 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -118,6 +118,9 @@ bool take_heading_control = false;
bool force_forward = false;
+bool guidance_indi_airspeed_filtering = false;
+
+
struct FloatVect3 sp_accel = {0.0,0.0,0.0};
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
@@ -148,6 +151,10 @@ static void guidance_indi_filter_thrust(void);
#endif
#endif
+#ifndef GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF
+#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5
+#endif
+
#ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD
#define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0
#endif
@@ -175,6 +182,7 @@ Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
Butterworth2LowPass thrust_filt;
Butterworth2LowPass accely_filt;
+Butterworth2LowPass guidance_indi_airspeed_filt;
struct FloatVect2 desired_airspeed;
@@ -204,6 +212,7 @@ float v_gih[3];
// Filters
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
+float guidance_indi_airspeed_filt_cutoff = GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF;
float guidance_indi_hybrid_heading_sp = 0.f;
struct FloatEulers guidance_euler_cmd;
@@ -310,6 +319,9 @@ void guidance_indi_init(void)
init_butterworth_2_low_pass(&pitch_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);
+
+ float tau_guidance_indi_airspeed = 1.0/(2.0*M_PI*guidance_indi_airspeed_filt_cutoff);
+ init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
#if GUIDANCE_INDI_HYBRID_USE_WLS
for (int8_t i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
@@ -349,6 +361,9 @@ 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);
+
+ float tau_guidance_indi_airspeed = 1.0/(2.0*M_PI*guidance_indi_airspeed_filt_cutoff);
+ init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
}
#include "firmwares/rotorcraft/navigation.h"
@@ -575,6 +590,9 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
float airspeed = 0.f;
#else
float airspeed = stateGetAirspeed_f();
+ if (guidance_indi_airspeed_filtering) {
+ airspeed = guidance_indi_airspeed_filt.o[0];
+ }
#endif
struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
@@ -785,6 +803,9 @@ 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);
+
+ float airspeed = stateGetAirspeed_f();
+ update_butterworth_2_low_pass(&guidance_indi_airspeed_filt, airspeed);
}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
index 4d46131beb1..5c13120bb21 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
@@ -103,6 +103,6 @@ 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;
#endif /* GUIDANCE_INDI_HYBRID_H */