From 3c0ffdf063b5dceae38718079e15a864d0be22cb Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 2 May 2024 15:42:41 -0600 Subject: [PATCH 1/3] Initialized yaw damper implementation. --- rosplane/include/controller_base.hpp | 1 + .../include/controller_successive_loop.hpp | 6 ++- rosplane/src/controller_successive_loop.cpp | 38 ++++++++++++++++--- 3 files changed, 38 insertions(+), 7 deletions(-) diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 04b4bc2..28ee6d1 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -50,6 +50,7 @@ class controller_base : public rclcpp::Node controller_base(); protected: + /** * This struct holds all of the inputs to the control algorithm. */ diff --git a/rosplane/include/controller_successive_loop.hpp b/rosplane/include/controller_successive_loop.hpp index 2693ff2..78c72ed 100644 --- a/rosplane/include/controller_successive_loop.hpp +++ b/rosplane/include/controller_successive_loop.hpp @@ -214,8 +214,12 @@ class controller_successive_loop : public controller_state_machine // float ct_error_; // float ct_integrator_; // float ct_differentiator_; + float yaw_damper(float r, const struct params_s ¶ms); - /** + float delta_r_delay; + float r_delay; + + /** * Saturate a given value to a maximum or minimum of the limits. * @param value The value to saturate. * @param up_limit The maximum the value can take on. diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index fc48531..46a173e 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -1,6 +1,7 @@ #include "controller_successive_loop.hpp" #include "iostream" +#include #include namespace rosplane @@ -84,8 +85,8 @@ void controller_successive_loop::alt_hold_lateral_control(const struct input_s & // Set rudder command to zero, can use cooridinated_turn_hold if implemented. // Find commanded roll angle in order to achieve commanded course. // Find aileron deflection required to acheive required roll angle. - output.delta_r = 0; //cooridinated_turn_hold(input.beta, params) - output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r); + output.delta_r = 0.0; // yaw_damper(input.r, params); //cooridinated_turn_hold(input.beta, params) + output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params); if (roll_tuning_debug_override) { output.phi_c = tuning_debug_override_msg_.phi_c; } @@ -116,8 +117,8 @@ void controller_successive_loop::climb_lateral_control(const struct input_s & in { // Maintain straight flight while gaining altitude. output.phi_c = 0; - output.delta_a = roll_hold(output.phi_c, input.phi, input.p); - output.delta_r = 0; + output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params); + output.delta_r = 0.0; //yaw_damper(input.r, params); } void controller_successive_loop::climb_longitudinal_control(const struct input_s & input, @@ -139,7 +140,7 @@ void controller_successive_loop::take_off_lateral_control(const struct input_s & struct output_s & output) { // In the take-off zone maintain level straight flight by commanding a roll angle of 0 and rudder of 0. - output.delta_r = 0; + output.delta_r = 0.0; output.phi_c = 0; output.delta_a = roll_hold(output.phi_c, input.phi, input.p); } @@ -222,6 +223,7 @@ float controller_successive_loop::course_hold(float chi_c, float chi, float phi_ float phi_c = sat(up + ui + ud + phi_ff, max_roll * 3.14 / 180.0, -max_roll * 3.14 / 180.0); + if (fabs(c_ki) >= 0.00001) { float phi_c_unsat = up + ui + ud + phi_ff; c_integrator_ = c_integrator_ + (Ts / c_ki) * (phi_c - phi_c_unsat); @@ -366,7 +368,31 @@ float controller_successive_loop::altitude_hold_control(float h_c, float h) return theta_c; } -//float controller_successive_loop::cooridinated_turn_hold(float v, const params_s ¶ms, float Ts) +float controller_successive_loop::yaw_damper(float r, const struct params_s ¶ms) +{ + + + float Ts = 1.0/params.frequency; + + float n0 = 0.0; + float n1 = 1.0; + float d0 = params.y_pwo; + float d1 = 1.0; + + float b0 = -params.y_kr * (2.0 * n1 - Ts * n0) / (2.0 * d1 + Ts * d0); + float b1 = params.y_kr * (2.0 * n1 + Ts * n0) / (2.0 * d1 + Ts * d0); + float a0 = (2.0 * d1 - Ts * d0) / (2.0 * d1 + Ts * d0); + + + float delta_r = sat(a0 * r_delay + b1 * r + b0 * r_delay, -params.max_r, params.max_r); + + r_delay = r; + delta_r_delay = delta_r; + + return delta_r; +} + +//float controller_successive_loop::cooridinated_turn_hold(float v, const params_s ¶ms) //{ // //todo finish this if you want... // return 0; From fcc609958dcaff4677fc5d8d2ce23e020a3ce39b Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Fri, 10 May 2024 21:16:56 -0600 Subject: [PATCH 2/3] Fixed typos in the yaw_damper. Added warning for saturation. --- .../include/controller_successive_loop.hpp | 2 +- rosplane/src/controller_successive_loop.cpp | 35 ++++++++++++------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/rosplane/include/controller_successive_loop.hpp b/rosplane/include/controller_successive_loop.hpp index 78c72ed..fbd692c 100644 --- a/rosplane/include/controller_successive_loop.hpp +++ b/rosplane/include/controller_successive_loop.hpp @@ -214,7 +214,7 @@ class controller_successive_loop : public controller_state_machine // float ct_error_; // float ct_integrator_; // float ct_differentiator_; - float yaw_damper(float r, const struct params_s ¶ms); + float yaw_damper(float r); float delta_r_delay; float r_delay; diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index 46a173e..939f224 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -85,8 +85,8 @@ void controller_successive_loop::alt_hold_lateral_control(const struct input_s & // Set rudder command to zero, can use cooridinated_turn_hold if implemented. // Find commanded roll angle in order to achieve commanded course. // Find aileron deflection required to acheive required roll angle. - output.delta_r = 0.0; // yaw_damper(input.r, params); //cooridinated_turn_hold(input.beta, params) - output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params); + output.delta_r = yaw_damper(input.r); //cooridinated_turn_hold(input.beta, params) + output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r); if (roll_tuning_debug_override) { output.phi_c = tuning_debug_override_msg_.phi_c; } @@ -117,7 +117,7 @@ void controller_successive_loop::climb_lateral_control(const struct input_s & in { // Maintain straight flight while gaining altitude. output.phi_c = 0; - output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params); + output.delta_a = roll_hold(output.phi_c, input.phi, input.p); output.delta_r = 0.0; //yaw_damper(input.r, params); } @@ -368,23 +368,25 @@ float controller_successive_loop::altitude_hold_control(float h_c, float h) return theta_c; } -float controller_successive_loop::yaw_damper(float r, const struct params_s ¶ms) +float controller_successive_loop::yaw_damper(float r) { + int64_t frequency = params.get_int("frequency"); // Declared in controller_base + float y_pwo = params.get_double("y_pwo"); + float y_kr = params.get_double("y_kr"); + float max_r = 1.0; // TODO Add to params - - float Ts = 1.0/params.frequency; + float Ts = 1.0/frequency; float n0 = 0.0; float n1 = 1.0; - float d0 = params.y_pwo; + float d0 = y_pwo; float d1 = 1.0; - float b0 = -params.y_kr * (2.0 * n1 - Ts * n0) / (2.0 * d1 + Ts * d0); - float b1 = params.y_kr * (2.0 * n1 + Ts * n0) / (2.0 * d1 + Ts * d0); - float a0 = (2.0 * d1 - Ts * d0) / (2.0 * d1 + Ts * d0); - + float b0 = -y_kr * (2.0 * n1 - Ts * n0) / (2.0 * d1 + Ts * d0); + float b1 = y_kr * (2.0 * n1 + Ts * n0) / (2.0 * d1 + Ts * d0); + float a0 = y_kr * (2.0 * d1 - Ts * d0) / (2.0 * d1 + Ts * d0); - float delta_r = sat(a0 * r_delay + b1 * r + b0 * r_delay, -params.max_r, params.max_r); + float delta_r = -sat(a0 * delta_r_delay + b1 * r + b0 * r_delay, max_r, -max_r); r_delay = r; delta_r_delay = delta_r; @@ -403,6 +405,12 @@ float controller_successive_loop::sat(float value, float up_limit, float low_lim // Set to upper limit if larger than that limit. // Set to lower limit if smaller than that limit. // Otherwise, do not change the value. + + if (up_limit < 0.0) + { + RCLCPP_WARN_ONCE(this->get_logger(), "Upper limit in saturation function is negative."); + } + float rVal; if (value > up_limit) rVal = up_limit; else if (value < low_limit) @@ -462,6 +470,9 @@ void controller_successive_loop::declare_parameters() params.declare_param("a_kp", 0.015); params.declare_param("a_ki", 0.003); params.declare_param("a_kd", 0.0); + + params.declare_param("y_pwo", .6349); + params.declare_param("y_kr", .85137); } } // namespace rosplane From d4ac72e1b9a06c544872f22a097c88d1960ab249 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Sat, 18 May 2024 12:45:15 -0600 Subject: [PATCH 3/3] Added yaw_damper to the climb portion of flight. --- rosplane/src/controller_successive_loop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index 939f224..a1844f6 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -118,7 +118,7 @@ void controller_successive_loop::climb_lateral_control(const struct input_s & in // Maintain straight flight while gaining altitude. output.phi_c = 0; output.delta_a = roll_hold(output.phi_c, input.phi, input.p); - output.delta_r = 0.0; //yaw_damper(input.r, params); + output.delta_r = yaw_damper(input.r); } void controller_successive_loop::climb_longitudinal_control(const struct input_s & input,