From 4f81d4520151255d8b4196b5f13096455fbb273e Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Wed, 13 Jul 2022 17:54:08 +0200 Subject: [PATCH] TESTING CODE --- .../FixedwingPositionINDIControl.cpp | 10 ++++++---- .../FixedwingPositionINDIControl.hpp | 1 + .../fw_dyn_soar_control_params.c | 14 ++++++++++++++ 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index f4bc646ab0..ad82253a5f 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -137,6 +137,8 @@ FixedwingPositionINDIControl::parameters_update() _inertia(0,0) = _param_fw_inertia_roll.get(); _inertia(1,1) = _param_fw_inertia_pitch.get(); _inertia(2,2) = _param_fw_inertia_yaw.get(); + _inertia(0,2) = _param_fw_inertia_rp.get(); + _inertia(2,0) = _param_fw_inertia_rp.get(); _mass = _param_fw_mass.get(); _area = _param_fw_wing_area.get(); _rho = _param_rho.get(); @@ -448,8 +450,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) // ======================================================================= bool error = false; - char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; - //char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; + //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; + char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; //PX4_ERR(home_dir); strcat(home_dir,filename); FILE* fp = fopen(home_dir, "r"); @@ -1276,12 +1278,12 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // ============================================================== Vector3f vel_air = _vel - _wind_estimate; Vector3f vel_normalized = vel_air.normalized(); - Vector3f acc_normalized = _acc.normalized(); + Vector3f acc_normalized = acc_filtered.normalized(); // compute ideal angular velocity Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); Vector3f omega_turn_ref; // constuct acc perpendicular to flight path - Vector3f acc_perp = _acc - (_acc*vel_normalized)*vel_normalized; + Vector3f acc_perp = acc_filtered - (acc_filtered*vel_normalized)*vel_normalized; if (_airspeed_valid&&_airspeed>_stall_speed) { omega_turn_ref = sqrtf(acc_perp*acc_perp) / (_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); //PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2))); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 5e1e5012b1..5fd2dac7ce 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -166,6 +166,7 @@ private: (ParamFloat) _param_fw_inertia_roll, (ParamFloat) _param_fw_inertia_pitch, (ParamFloat) _param_fw_inertia_yaw, + (ParamFloat) _param_fw_inertia_rp, (ParamFloat) _param_fw_mass, (ParamFloat) _param_fw_wing_area, (ParamFloat) _param_rho, 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 12a8922342..683555a316 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 @@ -52,6 +52,20 @@ PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f); */ PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f); +/** + * inertia tensor term in body xz-axis (roll-yaw coupling) + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min -0.5 + * @max 0 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f); + /** * total takeoff mass *