fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 13:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Switch origin to SITL position

Browse Source 
TODO: Fix problems with NANs
This commit is contained in:
JaeyoungLim
2025-07-08 16:50:30 +09:00
parent f4a2e0db40
commit 10d094ae97
3 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
+6 -4
View File
@@ -102,7 +102,7 @@ FixedwingPositionINDIControl::init()
} }
// initialize in manual feedthrough // initialize in manual feedthrough
_switch_manual = true; _switch_manual = false;
// fix sitl mode on startup (no switch at runtime) // fix sitl mode on startup (no switch at runtime)
_switch_sitl = _param_switch_sitl.get(); _switch_sitl = _param_switch_sitl.get();
@@ -824,6 +824,7 @@ FixedwingPositionINDIControl::Run()
Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f); Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f);
float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f)); float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f));
Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU 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 vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU
Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU)
Quatf q = _get_attitude_ref(t_ref,T); 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, // 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 // 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(0) = _lp_filter_ctrl0[0].apply(f_command(0));
f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1)); // f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1));
f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2)); // 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; _f_command = f_command;
// limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa)
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
+1 -1
View File
@@ -385,7 +385,7 @@ private:
// controler switches // controler switches
// ================== // ==================
// controller mode // controller mode
bool _switch_manual; bool _switch_manual{false};
// soaring mode // soaring mode
bool _switch_cl_soaring; bool _switch_cl_soaring;
// force limit // force limit
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
+2 -2
View File
@@ -475,7 +475,7 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f);
* @increment 0.0000001 * @increment 0.0000001
* @group FW DYN SOAR Control * @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) * longitude of trajectory start point (WGS84)
@@ -487,7 +487,7 @@ PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f);
* @increment 0.0000001 * @increment 0.0000001
* @group FW DYN SOAR Control * @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) * altitude of trajectory start point (WGS84)