From 1e73cdc4d2554273e1b5438af671ba971aaea662 Mon Sep 17 00:00:00 2001 From: Marvin Harms Date: Mon, 20 Jun 2022 08:46:04 +0200 Subject: [PATCH] added turn coordination for rudder deflection --- .../FixedwingPositionINDIControl.cpp | 51 +++++++++++++++---- .../FixedwingPositionINDIControl.hpp | 10 +++- .../fw_dyn_soar_control_params.c | 20 ++++---- src/modules/logger/logged_topics.cpp | 1 + 4 files changed, 60 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index e33847543b..324ced6dbf 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -407,8 +407,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"); @@ -458,8 +458,8 @@ FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) } row++; } + fclose(fp); } - fclose(fp); // ======================================================================= @@ -610,8 +610,8 @@ FixedwingPositionINDIControl::Run() // ===================== // compute control input // ===================== - Vector3f ctrl = _compute_NDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); - Vector3f ctrl1 = _compute_NDI_stage_2(ctrl); + Vector3f ctrl = _compute_INDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + Vector3f ctrl1 = _compute_INDI_stage_2(ctrl); // ============================ // compute actuator deflections @@ -725,6 +725,17 @@ FixedwingPositionINDIControl::Run() _soaring_controller_heartbeat.heartbeat = hrt_absolute_time(); _soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat); + // ==================== + // publish debug values + // ==================== + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + Vector3f vel_body = R_bi*_vel; + _slip = atan2f(vel_body(1), vel_body(0)); + _debug_value.timestamp = hrt_absolute_time(); + _debug_value.value = _slip; + _debug_value_pub.publish(_debug_value); + perf_end(_loop_perf); } @@ -1023,7 +1034,7 @@ FixedwingPositionINDIControl::_get_attitude(Vector3f vel, Vector3f f) } Vector3f -FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref) +FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref) { Dcmf R_ib(_att); Dcmf R_bi(R_ib.transpose()); @@ -1150,11 +1161,31 @@ FixedwingPositionINDIControl::_compute_NDI_stage_1(Vector3f pos_ref, Vector3f ve } + // ============================================================== + // overwrite rudder rot_acc_command with turn coordination values + // ============================================================== + Vector3f vel_air = _vel - _wind_estimate; + Vector3f vel_normalized = vel_air.normalized(); + Vector3f f = _mass*_acc; + Vector3f f_normalized = f.normalized(); + Vector3f omega_turn_ref_normalized = vel_normalized.cross(f_normalized); + Vector3f omega_turn_ref; + if (_airspeed_valid&&_airspeed>5.0f) { + omega_turn_ref = sqrtf(_acc*_acc) / (_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))); + } + else { + omega_turn_ref = sqrtf(_acc*_acc) / (5.f) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_ERR("No valid airspeed message detected or airspeed to low"); + } + + rot_acc_command(2) = _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2)); + return rot_acc_command; } Vector3f -FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) +FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) { // compute velocity in body frame Dcmf R_ib(_att); @@ -1188,9 +1219,9 @@ FixedwingPositionINDIControl::_compute_NDI_stage_2(Vector3f ctrl) 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); - // TODO: tune feedback turn coordination - float turn_coordination = 0.f*vel_body(1)/(powf(vel_body(0),2)+0.0001f); - deflection(2) += turn_coordination; + // overwrite rudder deflection with NDI turn coordination (no INDI) + Vector3f moment_ref = _inertia*ctrl + _omega.cross(_inertia*_omega); + deflection(2) = (moment_ref(2) + _k_d_yaw*q*_omega(2)) / fmaxf((_k_rud*q),0.0001f); return deflection; } diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp index 56ab469645..fffe368853 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -133,6 +134,7 @@ private: uORB::Publication _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)}; uORB::Publication _soaring_controller_position_pub{ORB_ID(soaring_controller_position)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; // Message structs vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; @@ -153,6 +155,7 @@ private: soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos + debug_value_s _debug_value{}; // slip angle // parameter struct DEFINE_PARAMETERS( @@ -263,11 +266,14 @@ private: Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel - Vector3f _compute_NDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); - Vector3f _compute_NDI_stage_2(Vector3f ctrl); + Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); + Vector3f _compute_INDI_stage_2(Vector3f ctrl); Vector3f _apply_LP_filter(Vector3f new_input, Vector &old_input, Vector &old_output); Vector3f _compute_actuator_deflections(Vector3f ctrl); + // yaw controller + ECL_YawController _yaw_ctrl; + // control variables Vector _basis_coeffs_x = {}; // coefficients of the current path Vector _basis_coeffs_y = {}; // coefficients of the current path 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 067bee9f7b..9e912c97bb 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 @@ -436,8 +436,8 @@ PARAM_DEFINE_FLOAT(DS_K_W_YAW, 1.0f); * @unit * @min 0 * @max 100 - * @decimal 2 - * @increment 0.01 + * @decimal 3 + * @increment 0.001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.1f); @@ -460,8 +460,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.03f); * @unit * @min 0 * @max 100 - * @decimal 2 - * @increment 0.01 + * @decimal 4 + * @increment 0.0001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f); @@ -472,8 +472,8 @@ PARAM_DEFINE_FLOAT(DS_K_ACT_YAW, 0.1f); * @unit * @min 0 * @max 100 - * @decimal 2 - * @increment 0.01 + * @decimal 3 + * @increment 0.001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f); @@ -484,8 +484,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.02f); * @unit * @min 0 * @max 100 - * @decimal 2 - * @increment 0.01 + * @decimal 3 + * @increment 0.001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f); @@ -496,8 +496,8 @@ PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.01f); * @unit * @min 0 * @max 100 - * @decimal 2 - * @increment 0.01 + * @decimal 4 + * @increment 0.0001 * @group FW DYN SOAR Control */ PARAM_DEFINE_FLOAT(DS_K_DAMP_YAW, 0.0f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 725abd019b..f035c83864 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -124,6 +124,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_actuator_setpoint", 20); add_topic("soaring_controller_position", 50); add_topic("soaring_controller_position_setpoint", 50); + add_topic("debug_value", 50); // multi topics add_topic_multi("actuator_outputs", 100, 3);