diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 091242a3c6..512c2ece9e 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/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; diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index e384678572..a49cae3685 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -211,7 +211,9 @@ private: // RC feedthrough params (ParamInt) _param_switch_manual, // force saturation - (ParamInt) _param_switch_saturation + (ParamInt) _param_switch_saturation, + // command filtering + (ParamInt) _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. diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index 683555a316..f9a19dd97a 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/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); \ No newline at end of file +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); \ No newline at end of file