From b4555c6e749d6604f354e676532460309862e3f5 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Sat, 9 Jul 2022 18:06:29 +0200 Subject: [PATCH] fixed error in high-level INDI --- .../FixedwingPositionINDIControl.cpp | 36 +++++++++++++------ .../FixedwingPositionINDIControl.hpp | 8 ++++- .../fw_dyn_soar_control_params.c | 20 +++++++++-- 3 files changed, 50 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 272faca42a..728ed480bd 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -171,6 +171,8 @@ FixedwingPositionINDIControl::parameters_update() _switch_manual = _param_switch_manual.get(); + _switch_saturation = _param_switch_saturation.get(); + // sanity check parameters // TODO: include sanity check @@ -1093,7 +1095,8 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // ========================================= Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-acc_filtered)) + acc_ref; // add gravity - acc_command(2) += 9.81f; + //acc_command(2) += 9.81f; + //acc_filtered(2) += 9.81f; // ================================== // compute expected aerodynamic force @@ -1106,17 +1109,28 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v float factor = -0.5f*_rho*_area*sqrtf(vel_body*vel_body); Vector3f w_x = vel_body; Vector3f w_z = w_x.cross(Vector3f{0.f,1.f,0.f}); - f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)) + Vector3f{0.f,0.f,-_mass*9.81f}; + f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)); // apply LP filter to force Vector3f f_current_filtered; f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); - // =============================== - // get force comand in world frame - // =============================== + // ================================ + // get force command in world frame + // ================================ Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered; + // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 12° aoa) + //PX4_INFO("force current, command: \t%.2f\t%.2f", (double)sqrtf(f_current_filtered*f_current_filtered), (double)sqrtf(f_command*f_command)); + + if (_switch_saturation){ + float f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.2f); + float f_now = sqrtf(f_command*f_command); + if (f_now>f_max){ + f_command = f_max/f_now * f_command; + } + } + // ========================================================================== // get required attitude (assuming we can fly the target velocity), and error @@ -1210,10 +1224,9 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v // ============================================================== Vector3f vel_air = _vel - _wind_estimate; Vector3f vel_normalized = vel_air.normalized(); - Vector3f f = _mass*(_acc); - Vector3f f_normalized = f.normalized(); + Vector3f acc_normalized = _acc.normalized(); // compute ideal angular velocity - Vector3f omega_turn_ref_normalized = vel_normalized.cross(f_normalized); + 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; @@ -1227,10 +1240,12 @@ FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f v } // transform rate vector to body frame - omega_turn_ref = R_bi*omega_turn_ref; + //omega_turn_ref = R_bi*omega_turn_ref; // not really a accel command, rather a FF-P command - rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(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)); + //PX4_INFO("omega turn ref: \t%.2f\t%.2f\t%.2f", (double)omega_turn_ref(0), (double)omega_turn_ref(1), (double)omega_turn_ref(2)); + //PX4_INFO("omega turn : \t%.2f\t%.2f\t%.2f", (double)_omega(0), (double)_omega(1), (double)_omega(2)); return rot_acc_command; } @@ -1268,7 +1283,6 @@ FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) deflection(0) = (moment_command(0) + _k_d_roll*q*omega_filtered(0))/fmaxf((_k_ail*q),0.0001f); deflection(1) = (moment_command(1) + _k_d_pitch*q*omega_filtered(1))/fmaxf((_k_ele*q),0.0001f); deflection(2) = (moment_command(2) + _k_d_yaw*q*omega_filtered(2))/fmaxf((_k_rud*q),0.0001f); - // overwrite rudder deflection with NDI turn coordination (no INDI) Vector3f moment_ref = _inertia*ctrl + omega_filtered.cross(_inertia*omega_filtered); deflection(2) = (moment_ref(2) + _k_d_yaw*q*omega_filtered(2)) / fmaxf((_k_rud*q),0.0001f); diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 78cc3d1f56..00fefb4a5b 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -210,7 +210,9 @@ private: // thrust params (ParamFloat) _param_thrust, // RC feedthrough params - (ParamInt) _param_switch_manual + (ParamInt) _param_switch_manual, + // force saturation + (ParamInt) _param_switch_saturation ) @@ -340,6 +342,8 @@ private: float _thrust; // controller mode bool _switch_manual; + // force limit + bool _switch_saturation; bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. @@ -363,6 +367,8 @@ private: Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame Vector3f _zero_crossing_local_pos; // vector denoting the zero crossing of the trajectories in NED frame + Vector3f _f_command_filtered {}; + Vector3f _m_command_filtered {}; }; 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 c92970b6ed..b2658d6c58 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 @@ -184,7 +184,7 @@ PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f); /** * stall speed of the aircraft * - * @unit rad + * @unit m/s * @min 5 * @max 10 * @decimal 1 @@ -539,4 +539,20 @@ PARAM_DEFINE_FLOAT(DS_THRUST, 0); * @increment 1 * @group FW DYN SOAR Control */ -PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0); \ No newline at end of file +PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 0); + +// ====================================================== +// ============= controller force saturation ============ +// ====================================================== + +/** + * integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation + * + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_SAT, 0); \ No newline at end of file