From f2877ce585b9995a76a4f7faaf02b6081eeddd70 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Wed, 13 Jul 2022 17:33:39 +0200 Subject: [PATCH] Replace rate controller with RateControlLibrary This commit makes the fw attitude controller take share the rate controller as a library with the mc_rate_control module --- src/lib/CMakeLists.txt | 1 + .../rate_control}/CMakeLists.txt | 6 +- .../rate_control/rate_control.cpp} | 2 +- .../rate_control/rate_control.hpp} | 2 +- .../rate_control/rate_control_test.cpp} | 2 +- src/modules/fw_att_control/CMakeLists.txt | 2 + .../FixedwingAttitudeControl.cpp | 69 ++++++++++--------- .../FixedwingAttitudeControl.hpp | 7 ++ src/modules/fw_att_control/ecl_controller.h | 2 - .../fw_att_control/ecl_pitch_controller.cpp | 65 ++--------------- .../fw_att_control/ecl_pitch_controller.h | 2 - .../fw_att_control/ecl_roll_controller.cpp | 62 ++--------------- .../fw_att_control/ecl_roll_controller.h | 2 - .../fw_att_control/ecl_wheel_controller.h | 4 +- .../fw_att_control/ecl_yaw_controller.cpp | 67 ++---------------- .../fw_att_control/ecl_yaw_controller.h | 2 - src/modules/mc_rate_control/CMakeLists.txt | 2 - .../MulticopterRateControl.hpp | 3 +- 18 files changed, 70 insertions(+), 232 deletions(-) rename src/{modules/mc_rate_control/RateControl => lib/rate_control}/CMakeLists.txt (94%) rename src/{modules/mc_rate_control/RateControl/RateControl.cpp => lib/rate_control/rate_control.cpp} (99%) rename src/{modules/mc_rate_control/RateControl/RateControl.hpp => lib/rate_control/rate_control.hpp} (99%) rename src/{modules/mc_rate_control/RateControl/RateControlTest.cpp => lib/rate_control/rate_control_test.cpp} (97%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index bb27f0eccb..7c56c4a8f1 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -59,6 +59,7 @@ add_subdirectory(perf) add_subdirectory(pid) add_subdirectory(pid_design) add_subdirectory(pwm) +add_subdirectory(rate_control) add_subdirectory(rc) add_subdirectory(sensor_calibration) add_subdirectory(slew_rate) diff --git a/src/modules/mc_rate_control/RateControl/CMakeLists.txt b/src/lib/rate_control/CMakeLists.txt similarity index 94% rename from src/modules/mc_rate_control/RateControl/CMakeLists.txt rename to src/lib/rate_control/CMakeLists.txt index b536a7db09..f95b133eea 100644 --- a/src/modules/mc_rate_control/RateControl/CMakeLists.txt +++ b/src/lib/rate_control/CMakeLists.txt @@ -32,11 +32,11 @@ ############################################################################ px4_add_library(RateControl - RateControl.cpp - RateControl.hpp + rate_control.cpp + rate_control.hpp ) target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(RateControl PRIVATE mathlib) -px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl) +px4_add_unit_gtest(SRC rate_control_test.cpp LINKLIBS RateControl) diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/lib/rate_control/rate_control.cpp similarity index 99% rename from src/modules/mc_rate_control/RateControl/RateControl.cpp rename to src/lib/rate_control/rate_control.cpp index bf659315f8..77b776ba11 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/lib/rate_control/rate_control.cpp @@ -35,7 +35,7 @@ * @file RateControl.cpp */ -#include +#include "rate_control.hpp" #include using namespace matrix; diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/lib/rate_control/rate_control.hpp similarity index 99% rename from src/modules/mc_rate_control/RateControl/RateControl.hpp rename to src/lib/rate_control/rate_control.hpp index 96698bc565..1db18ee092 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/lib/rate_control/rate_control.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file RateControl.hpp + * @file rate_control.hpp * * PID 3 axis angular rate / angular velocity control. */ diff --git a/src/modules/mc_rate_control/RateControl/RateControlTest.cpp b/src/lib/rate_control/rate_control_test.cpp similarity index 97% rename from src/modules/mc_rate_control/RateControl/RateControlTest.cpp rename to src/lib/rate_control/rate_control_test.cpp index 791fb2072d..bd6238b6dd 100644 --- a/src/modules/mc_rate_control/RateControl/RateControlTest.cpp +++ b/src/lib/rate_control/rate_control_test.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ #include -#include +#include using namespace matrix; diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 2cfa1435ba..de002d7617 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( MODULE modules__fw_att_control MAIN fw_att_control @@ -44,5 +45,6 @@ px4_add_module( ecl_yaw_controller.cpp DEPENDS px4_work_queue + RateControl SlewRate ) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9a2215d86d..a794726f83 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -34,6 +34,8 @@ #include "FixedwingAttitudeControl.hpp" using namespace time_literals; +using namespace matrix; + using math::constrain; using math::interpolate; using math::radians; @@ -81,23 +83,9 @@ FixedwingAttitudeControl::parameters_update() { /* pitch control parameters */ _pitch_ctrl.set_time_constant(_param_fw_p_tc.get()); - _pitch_ctrl.set_k_p(_param_fw_pr_p.get()); - _pitch_ctrl.set_k_i(_param_fw_pr_i.get()); - _pitch_ctrl.set_k_ff(_param_fw_pr_ff.get()); - _pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get()); /* roll control parameters */ _roll_ctrl.set_time_constant(_param_fw_r_tc.get()); - _roll_ctrl.set_k_p(_param_fw_rr_p.get()); - _roll_ctrl.set_k_i(_param_fw_rr_i.get()); - _roll_ctrl.set_k_ff(_param_fw_rr_ff.get()); - _roll_ctrl.set_integrator_max(_param_fw_rr_imax.get()); - - /* yaw control parameters */ - _yaw_ctrl.set_k_p(_param_fw_yr_p.get()); - _yaw_ctrl.set_k_i(_param_fw_yr_i.get()); - _yaw_ctrl.set_k_ff(_param_fw_yr_ff.get()); - _yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get()); /* wheel control parameters */ _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); @@ -106,6 +94,19 @@ FixedwingAttitudeControl::parameters_update() _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); + const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get()); + const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get()); + const Vector3f rate_d = Vector3f(0.0f, 0.0f, 0.0f); + + _rate_control.setGains(rate_p, rate_i, rate_d); + + _rate_control.setIntegratorLimit( + Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get())); + + return PX4_OK; } @@ -312,6 +313,11 @@ void FixedwingAttitudeControl::Run() float rollspeed = angular_velocity.xyz[0]; float pitchspeed = angular_velocity.xyz[1]; float yawspeed = angular_velocity.xyz[2]; + const Vector3f rates(rollspeed, pitchspeed, yawspeed); + + vehicle_angular_acceleration_s angular_acceleration{}; + _vehicle_angular_acceleration_sub.copy(&angular_acceleration); + const Vector3f angular_accel{angular_acceleration.xyz}; if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode @@ -531,9 +537,8 @@ void FixedwingAttitudeControl::Run() } /* Update input data for rate controllers */ - control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); - control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); - control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); + Vector3f rates_setpoint = Vector3f(_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), + _yaw_ctrl.get_desired_rate()); const hrt_abstime now = hrt_absolute_time(); autotune_attitude_control_status_s pid_autotune; @@ -547,11 +552,13 @@ void FixedwingAttitudeControl::Run() && ((now - pid_autotune.timestamp) < 1_s)) { bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp); + rates_setpoint = rates_setpoint + bodyrate_ff; } } /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0)); + const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed); + float roll_u = att_control(0) * _airspeed_scaling * _airspeed_scaling; _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; @@ -559,7 +566,7 @@ void FixedwingAttitudeControl::Run() _roll_ctrl.reset_integrator(); } - float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1)); + float pitch_u = att_control(1) * _airspeed_scaling * _airspeed_scaling; _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; @@ -576,7 +583,7 @@ void FixedwingAttitudeControl::Run() yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get(); } else { - yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2)); + yaw_u = att_control(2) * _airspeed_scaling * _airspeed_scaling; } _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; @@ -587,7 +594,7 @@ void FixedwingAttitudeControl::Run() } if (!PX4_ISFINITE(yaw_u)) { - _yaw_ctrl.reset_integrator(); + // _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } @@ -626,19 +633,15 @@ void FixedwingAttitudeControl::Run() } else { vehicle_rates_setpoint_poll(); - _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); - _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); - _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); + const Vector3f rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + const Vector3f att_control = _rate_control.update(rates, rates_setpoint, angular_accel, dt, _landed); - float roll_u = _roll_ctrl.control_bodyrate(dt, control_input); - _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - - float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input); - _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : - trim_pitch; - - float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input); - _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; + _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control( + 0) + trim_roll : trim_roll; + _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(att_control(1))) ? att_control( + 1) + trim_pitch : trim_pitch; + _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(att_control(2))) ? att_control( + 2) + trim_yaw : trim_yaw; _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index d71d6f63c3..8b71b52ff2 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -31,6 +31,10 @@ * ****************************************************************************/ +#pragma once + +#include + #include #include "ecl_pitch_controller.h" #include "ecl_roll_controller.h" @@ -61,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +125,7 @@ private: uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; @@ -235,6 +241,7 @@ private: ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; + RateControl _rate_control; ///< class for rate control calculations /** * @brief Update flap control setting diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index f025890f7c..10f525952f 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/src/modules/fw_att_control/ecl_controller.h @@ -80,8 +80,6 @@ public: virtual ~ECL_Controller() = default; virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0; - virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) = 0; - virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0; /* Setters */ void set_time_constant(float time_constant); diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index d1a43a9412..9b492e45de 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -58,66 +58,11 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; + float euler_rate_setpoint = pitch_error / _tc; + + /* Transform setpoint to body angular rates (jacobian) */ + _rate_setpoint = cosf(ctl_data.roll) * euler_rate_setpoint + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; return _rate_setpoint; } - -float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.body_y_rate) && - PX4_ISFINITE(ctl_data.body_z_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { - - return math::constrain(_last_output, -1.0f, 1.0f); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; - - if (!ctl_data.lock_integrator && _k_i > 0.0f) { - - /* Integral term scales with 1/IAS^2 */ - float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); - } - - /* Apply PI rate controller and store non-limited output */ - /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ - _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + _integrator; - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) -{ - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(dt, ctl_data); -} diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 3ecc468a29..4eaf2e2f84 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -61,8 +61,6 @@ public: ~ECL_PitchController() = default; float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; - float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override; - float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; /* Additional Setters */ void set_max_rate_pos(float max_rate_pos) diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index 8d20a72c72..e63f53574b 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -56,64 +56,10 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData float roll_error = ctl_data.roll_setpoint - ctl_data.roll; /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = roll_error / _tc; + float euler_rate_setpoint = roll_error / _tc; + + /* Transform setpoint to body angular rates (jacobian) */ + _rate_setpoint = euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; return _rate_setpoint; } - -float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.body_x_rate) && - PX4_ISFINITE(ctl_data.body_z_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { - - return math::constrain(_last_output, -1.0f, 1.0f); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; - - if (!ctl_data.lock_integrator && _k_i > 0.0f) { - - /* Integral term scales with 1/IAS^2 */ - float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); - } - - /* Apply PI rate controller and store non-limited output */ - /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ - _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + _integrator; - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) -{ - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(dt, ctl_data); -} diff --git a/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/ecl_roll_controller.h index e7ea5d238b..2ca62489ad 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.h +++ b/src/modules/fw_att_control/ecl_roll_controller.h @@ -59,8 +59,6 @@ public: ~ECL_RollController() = default; float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; - float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override; - float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/modules/fw_att_control/ecl_wheel_controller.h b/src/modules/fw_att_control/ecl_wheel_controller.h index ddfab044df..7b84fe1b4e 100644 --- a/src/modules/fw_att_control/ecl_wheel_controller.h +++ b/src/modules/fw_att_control/ecl_wheel_controller.h @@ -60,9 +60,9 @@ public: float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; - float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + float control_bodyrate(const float dt, const ECL_ControlData &ctl_data); - float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override { (void)ctl_data; return 0; } + float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; } }; #endif // ECL_HEADING_CONTROLLER_H diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index 09716d84e9..84f105e195 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -82,8 +82,12 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData if (!inverted) { /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < - ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + float euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < + ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); + + /* Transform setpoint to body angular rates (jacobian) */ + _rate_setpoint = -sinf(ctl_data.roll) * euler_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * euler_rate_setpoint; } if (!PX4_ISFINITE(_rate_setpoint)) { @@ -93,62 +97,3 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData return _rate_setpoint; } - -float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.body_y_rate) && - PX4_ISFINITE(ctl_data.body_z_rate) && - PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { - - return math::constrain(_last_output, -1.0f, 1.0f); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; - - if (!ctl_data.lock_integrator && _k_i > 0.0f) { - - /* Integral term scales with 1/IAS^2 */ - float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - /* add and constrain */ - _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); - } - - /* Apply PI rate controller and store non-limited output */ - /* FF terms scales with 1/TAS and P,I with 1/IAS^2 */ - _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + - _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler - + _integrator; - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) -{ - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff; - - set_bodyrate_setpoint(_bodyrate_setpoint); - - return control_bodyrate(dt, ctl_data); -} diff --git a/src/modules/fw_att_control/ecl_yaw_controller.h b/src/modules/fw_att_control/ecl_yaw_controller.h index d197fbcdf3..4a8f1fb3cb 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.h +++ b/src/modules/fw_att_control/ecl_yaw_controller.h @@ -59,8 +59,6 @@ public: ~ECL_YawController() = default; float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; - float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override; - float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; protected: float _max_rate{0.0f}; diff --git a/src/modules/mc_rate_control/CMakeLists.txt b/src/modules/mc_rate_control/CMakeLists.txt index 758d56b7c8..849993e623 100644 --- a/src/modules/mc_rate_control/CMakeLists.txt +++ b/src/modules/mc_rate_control/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################ -add_subdirectory(RateControl) - px4_add_module( MODULE modules__mc_rate_control MAIN mc_rate_control diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index d3a0be95f1..64aaf90ce1 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -33,8 +33,7 @@ #pragma once -#include - +#include #include #include #include