From 10d094ae97646409cdf253cf2480dc7b2f6d839e Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Tue, 8 Jul 2025 16:30:04 +0900 Subject: [PATCH] Switch origin to SITL position TODO: Fix problems with NANs --- .../FixedwingPositionINDIControl.cpp | 10 ++++++---- .../FixedwingPositionINDIControl.hpp | 2 +- .../fw_dyn_soar_control/fw_dyn_soar_control_params.c | 4 ++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 5fa1da6f6b..4d4f17fb47 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -102,7 +102,7 @@ FixedwingPositionINDIControl::init() } // initialize in manual feedthrough - _switch_manual = true; + _switch_manual = false; // fix sitl mode on startup (no switch at runtime) _switch_sitl = _param_switch_sitl.get(); @@ -824,6 +824,7 @@ FixedwingPositionINDIControl::Run() Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f); float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f)); Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU + PX4_INFO("Reference Position: %f, %f, %f", double(pos_ref(0)), double(pos_ref(1)), double(pos_ref(2))); Vector3f vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) Quatf q = _get_attitude_ref(t_ref,T); @@ -1330,9 +1331,10 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // 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)); + // 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)); + // PX4_INFO(" - f_command - filtered: %f, %f, %f", double(f_command(0)), double(f_command(1)), double(f_command(2))); _f_command = f_command; // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 45de21e031..beb2a1bfeb 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -385,7 +385,7 @@ private: // controler switches // ================== // controller mode - bool _switch_manual; + bool _switch_manual{false}; // soaring mode bool _switch_cl_soaring; // force limit 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 52ab57b1a1..a1ea2e93fd 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 @@ -475,7 +475,7 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.397997f); /** * longitude of trajectory start point (WGS84) @@ -487,7 +487,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.546039f); /** * altitude of trajectory start point (WGS84)