From 258da619e0c2bd809088c80616c758f7d8cd9a96 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Tue, 21 Jun 2022 11:19:56 +0200 Subject: [PATCH] added FF term to turn coordination --- .../fw_dyn_soar_control/FixedwingPositionINDIControl.cpp | 2 +- .../fw_dyn_soar_control/fw_dyn_soar_control_params.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index d8ad550b5e..d2b8850b51 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -1183,7 +1183,7 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v } // not really a accel command, rather a FF-P command - rot_acc_command(2) = _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2)); + rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2) + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2)); return rot_acc_command; } 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 68a5c8ee3a..7f7907a62d 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 @@ -400,7 +400,7 @@ PARAM_DEFINE_FLOAT(DS_K_Q_PITCH, 10.0f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 2.0f); +PARAM_DEFINE_FLOAT(DS_K_Q_YAW, 0.5f); /** * roll gain of K_W (angular velocity error gain) @@ -528,7 +528,7 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3135000f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.4040182f); /** * longitude of trajectory start point (WGS84) @@ -540,7 +540,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3135000f); * @increment 0.0000001 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8101000f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.5101919f); /** * altitude of trajectory start point (WGS84) @@ -552,7 +552,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8101000f); * @increment 0.1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); +PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 540.0f); // ====================================================== // ============== loiter circle number =================