diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index 0414c9e30b..dd503140c4 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -41,7 +41,7 @@ px4_add_module( ecl_controller.cpp fw_pitch_controller.cpp fw_roll_controller.cpp - ecl_wheel_controller.cpp + fw_wheel_controller.cpp fw_yaw_controller.cpp DEPENDS px4_work_queue diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 320649e899..de03fdfe27 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -36,7 +36,7 @@ #include #include "fw_pitch_controller.h" #include "fw_roll_controller.h" -#include "ecl_wheel_controller.h" +#include "fw_wheel_controller.h" #include "fw_yaw_controller.h" #include #include @@ -162,7 +162,7 @@ private: RollController _roll_ctrl; PitchController _pitch_ctrl; YawController _yaw_ctrl; - ECL_WheelController _wheel_ctrl; + WheelController _wheel_ctrl; void parameters_update(); void vehicle_manual_poll(const float yaw_body); diff --git a/src/modules/fw_att_control/ecl_wheel_controller.cpp b/src/modules/fw_att_control/fw_wheel_controller.cpp similarity index 58% rename from src/modules/fw_att_control/ecl_wheel_controller.cpp rename to src/modules/fw_att_control/fw_wheel_controller.cpp index 869aaa22ca..e26d9a5d38 100644 --- a/src/modules/fw_att_control/ecl_wheel_controller.cpp +++ b/src/modules/fw_att_control/fw_wheel_controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,13 +32,11 @@ ****************************************************************************/ /** - * @file ecl_wheel_controller.cpp + * @file fw_wheel_controller.cpp * Implementation of a simple PID wheel controller for heading tracking. - * - * Authors and acknowledgements in header. */ -#include "ecl_wheel_controller.h" +#include "fw_wheel_controller.h" #include #include #include @@ -46,68 +44,56 @@ using matrix::wrap_pi; -float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) +float WheelController::control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.body_z_rate) && - PX4_ISFINITE(ctl_data.groundspeed) && - PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + if (!(PX4_ISFINITE(body_z_rate) && + PX4_ISFINITE(groundspeed) && + PX4_ISFINITE(groundspeed_scaler))) { - return math::constrain(_last_output, -1.0f, 1.0f); + return math::constrain(_last_output, -1.f, 1.f); } - /* input conditioning */ - float min_speed = 1.0f; + const float rate_error = _body_rate_setpoint - body_z_rate; - /* Calculate body angular rate error */ - const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error + if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s - if (_k_i > 0.0f && ctl_data.groundspeed > min_speed) { + float id = rate_error * dt * groundspeed_scaler; - float id = rate_error * dt * ctl_data.groundspeed_scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { + if (_last_output < -1.f) { /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); + id = math::max(id, 0.f); - } else if (_last_output > 1.0f) { + } else if (_last_output > 1.f) { /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); + id = math::min(id, 0.f); } - /* add and constrain */ _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); } /* Apply PI rate controller and store non-limited output */ - _last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + - ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator); + _last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler + + groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p + _integrator); - return math::constrain(_last_output, -1.0f, 1.0f); + return math::constrain(_last_output, -1.f, 1.f); } -float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data) +float WheelController::control_attitude(float yaw_setpoint, float yaw) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw))) { + if (!(PX4_ISFINITE(yaw_setpoint) && + PX4_ISFINITE(yaw))) { return _body_rate_setpoint; } - /* Calculate the error */ - float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw); + const float yaw_error = wrap_pi(yaw_setpoint - yaw); - /* Apply P controller: rate setpoint from current error and time constant */ - _euler_rate_setpoint = yaw_error / _tc; - _body_rate_setpoint = _euler_rate_setpoint; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix + _body_rate_setpoint = yaw_error / _tc; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix - /* limit the rate */ if (_max_rate > 0.01f) { - if (_body_rate_setpoint > 0.0f) { + if (_body_rate_setpoint > 0.f) { _body_rate_setpoint = (_body_rate_setpoint > _max_rate) ? _max_rate : _body_rate_setpoint; } else { diff --git a/src/modules/fw_att_control/ecl_wheel_controller.h b/src/modules/fw_att_control/fw_wheel_controller.h similarity index 60% rename from src/modules/fw_att_control/ecl_wheel_controller.h rename to src/modules/fw_att_control/fw_wheel_controller.h index dc77073d04..94b684aa57 100644 --- a/src/modules/fw_att_control/ecl_wheel_controller.h +++ b/src/modules/fw_att_control/fw_wheel_controller.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,44 +32,48 @@ ****************************************************************************/ /** - * @file ecl_wheel_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Andreas Antener - * - * Acknowledgements: - * - * The control design is based on a design - * by Paul Riseborough and Andrew Tridgell, 2013, - * which in turn is based on initial work of - * Jonathan Challinger, 2012. + * @file fw_wheel_controller.h + * Definition of a simple wheel controller. */ -#ifndef ECL_HEADING_CONTROLLER_H -#define ECL_HEADING_CONTROLLER_H +#ifndef FW_WHEEL_CONTROLLER_H +#define FW_WHEEL_CONTROLLER_H -#include "ecl_controller.h" - -class ECL_WheelController : - public ECL_Controller +class WheelController { public: - ECL_WheelController() = default; - ~ECL_WheelController() = default; + WheelController() = default; + ~WheelController() = default; /** * @brief Calculates wheel body rate setpoint. * - * @param dt Time step [s] - * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed) + * @param yaw_setpoint yaw setpoint [rad] + * @param yaw estimated yaw [rad] * @return Wheel body rate setpoint [rad/s] */ - float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; + float control_attitude(float yaw_setpoint, float yaw); - float control_bodyrate(const float dt, const ECL_ControlData &ctl_data); + float control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler); - float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; } + void set_time_constant(float time_constant) { _tc = time_constant; } + void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } + void set_k_ff(float k_ff) { _k_ff = k_ff; } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } + + void reset_integrator() { _integrator = 0.f; } + +private: + float _tc; + float _k_p; + float _k_i; + float _k_ff; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _body_rate_setpoint; }; -#endif // ECL_HEADING_CONTROLLER_H +#endif // FW_WHEEL_CONTROLLER_H