fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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:30:04 +09:00
parent f4a2e0db40
commit 10d094ae97
3 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp
View File

@ -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)

2
src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp
View File

@ -385,7 +385,7 @@ private:
// controler switches
// ==================
// controller mode
bool _switch_manual;
bool _switch_manual{false};
// soaring mode
bool _switch_cl_soaring;
// force limit

4
src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c
View File

@ -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)