improved controller smoothing & CONSISTENCY

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp

@@ -181,6 +181,8 @@ FixedwingPositionINDIControl::parameters_update()
 
     _switch_saturation = _param_switch_saturation.get();
 
+    _switch_filter = _param_switch_filter.get();
+
 
 	// sanity check parameters
     // TODO: include sanity check
@@ -1165,6 +1167,15 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
     // get force command in world frame
     // ================================
     Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered;
+
+    // ============================================================================================================
+    // apply some filtering to the force command. This introduces some time delay,
+    // which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller
+    // ============================================================================================================
+    f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0));
+    f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1));
+    f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2));
+
     // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 12° aoa)
     //PX4_INFO("force current, command: \t%.2f\t%.2f", (double)sqrtf(f_current_filtered*f_current_filtered), (double)sqrtf(f_command*f_command));
 
@@ -1215,7 +1226,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
     // =========================================
     // apply PD control law on the body attitude
     // =========================================
-    Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-omega_filtered) + alpha_ref;
+    Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref;
 
     // ==========================================
     // input meant for tuning the INDI controller
@@ -1269,7 +1280,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
         }
 
         // compute rot acc command
-        rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-omega_filtered);
+        rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-_omega);
         
     }
 
@@ -1305,9 +1316,11 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v
     // filter the stage 1 controller outputs to filter out high-frequency components.
     // This is desirable as the provided commands might be very noisy otherwise (not feasible)
     // =======================================================================================
-    rot_acc_command(0) = _lp_filter_ctrl1[0].apply(rot_acc_command(0));
-    rot_acc_command(1) = _lp_filter_ctrl1[1].apply(rot_acc_command(1));
-    rot_acc_command(2) = _lp_filter_ctrl1[2].apply(rot_acc_command(2));
+    if (_switch_filter) {
+        rot_acc_command(0) = _lp_filter_ctrl1[0].apply(rot_acc_command(0));
+        rot_acc_command(1) = _lp_filter_ctrl1[1].apply(rot_acc_command(1));
+        rot_acc_command(2) = _lp_filter_ctrl1[2].apply(rot_acc_command(2));
+    }
     
 
     return rot_acc_command;

src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp

@@ -211,7 +211,9 @@ private:
 		// RC feedthrough params
 		(ParamInt<px4::params::DS_SWITCH_MANUAL>) _param_switch_manual,
 		// force saturation
-		(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation
+		(ParamInt<px4::params::DS_SWITCH_SAT>) _param_switch_saturation,
+		// command filtering
+		(ParamInt<px4::params::DS_SWITCH_FILTER>) _param_switch_filter
 
 	)
 
@@ -306,6 +308,7 @@ private:
 	math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}};	// body rates
 	// smoothing filter to reject HF noise in control output
 	const float _cutoff_frequency_smoothing = 30.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz)
+	math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}};	// force command stage 1
 	math::LowPassFilter2p _lp_filter_ctrl1[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}};	// control output stage 1
 	// Low-Pass filters stage 2
 	const float _cutoff_frequency_2 = 30.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF"
@@ -350,6 +353,8 @@ private:
 	bool _switch_manual;
 	// force limit
 	bool _switch_saturation;
+	//
+	bool _switch_filter;
 
 	bool _airspeed_valid{false};				///< flag if a valid airspeed estimate exists
 	hrt_abstime _airspeed_last_valid{0};			///< last time airspeed was received. Used to detect timeouts.

src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c

@@ -547,4 +547,19 @@ PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0);
  * @increment 1
  * @group FW DYN SOAR Control
  */
-PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
+PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1);
+
+// ======================================================
+// ============= controller output filtering ============
+// ======================================================
+
+/**
+ * integer in {0,1} defining if the rotation acceleration command will get filtered before performing INDI
+ * @unit 
+ * @min 0
+ * @max 1
+ * @decimal 1
+ * @increment 1
+ * @group FW DYN SOAR Control
+ */
+PARAM_DEFINE_INT32(DS_SWITCH_FILTER, 0);