diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index d98b7600cb..e2c4f9b3f2 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -42,10 +42,7 @@ px4_add_module( FixedwingAttitudeControl.hpp ecl_controller.cpp - ecl_pitch_controller.cpp - ecl_roll_controller.cpp ecl_wheel_controller.cpp - ecl_yaw_controller.cpp DEPENDS px4_work_queue FWRateControl diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 6452bda7f9..c3d529087d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -52,10 +52,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : parameters_update(); // set initial maximum body rate setpoints - _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); + // _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); + // _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); + // _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); + // _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); _rate_ctrl_status_pub.advertise(); _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); @@ -81,26 +81,6 @@ FixedwingAttitudeControl::init() int 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()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); @@ -406,16 +386,16 @@ void FixedwingAttitudeControl::Run() const float airspeed = get_airspeed_and_update_scaling(); /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } + // if (_att_sp.roll_reset_integral) { + // _roll_ctrl.reset_integrator(); + // } - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } + // if (_att_sp.pitch_reset_integral) { + // _pitch_ctrl.reset_integrator(); + // } if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); + // _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } @@ -426,9 +406,9 @@ void FixedwingAttitudeControl::Run() || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { - _roll_ctrl.reset_integrator(); - _pitch_ctrl.reset_integrator(); - _yaw_ctrl.reset_integrator(); + // _roll_ctrl.reset_integrator(); + // _pitch_ctrl.reset_integrator(); + // _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } @@ -472,16 +452,17 @@ void FixedwingAttitudeControl::Run() if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { if (_vcontrol_mode.flag_control_attitude_enabled || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); + using math::radians; + // _roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get())); + // _pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get())); + // _pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get())); + // _yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get())); } else { - _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); - _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); - _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); - _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); + // _roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get())); + // _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); + // _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); + // _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); } } @@ -517,26 +498,21 @@ void FixedwingAttitudeControl::Run() trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get(); /* Run attitude controllers */ + Vector3f rates_setpoint; + if (_vcontrol_mode.flag_control_attitude_enabled) { if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_attitude(dt, control_input); - _pitch_ctrl.control_attitude(dt, control_input); + /* Run ATTITUDE controller */ + rates_setpoint = _attitude_control.update(Quatf(att.q)); if (wheel_control) { _wheel_ctrl.control_attitude(dt, control_input); - _yaw_ctrl.reset_integrator(); } else { // runs last, because is depending on output of roll and pitch attitude - _yaw_ctrl.control_attitude(dt, control_input); _wheel_ctrl.reset_integrator(); } - /* 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(); - const hrt_abstime now = hrt_absolute_time(); autotune_attitude_control_status_s pid_autotune; matrix::Vector3f bodyrate_ff; @@ -552,22 +528,28 @@ void FixedwingAttitudeControl::Run() } } + vehicle_angular_acceleration_s v_angular_acceleration{}; + // _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration); + const Vector3f angular_accel{v_angular_acceleration.xyz}; + /* 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); _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - if (!PX4_ISFINITE(roll_u)) { - _roll_ctrl.reset_integrator(); - } + // if (!PX4_ISFINITE(roll_u)) { + // _roll_ctrl.reset_integrator(); + // } - float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1)); + float pitch_u = att_control(1); _actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; - if (!PX4_ISFINITE(pitch_u)) { - _pitch_ctrl.reset_integrator(); - } + // if (!PX4_ISFINITE(pitch_u)) { + // _pitch_ctrl.reset_integrator(); + // } float yaw_u = 0.0f; @@ -575,7 +557,7 @@ void FixedwingAttitudeControl::Run() yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input); } else { - yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2)); + yaw_u = att_control(2); } _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; @@ -586,7 +568,7 @@ void FixedwingAttitudeControl::Run() } if (!PX4_ISFINITE(yaw_u)) { - _yaw_ctrl.reset_integrator(); + // _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } @@ -614,9 +596,9 @@ void FixedwingAttitudeControl::Run() * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ - _rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); - _rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); - _rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); + _rates_sp.roll = rates_setpoint(0); + _rates_sp.pitch = rates_setpoint(1); + _rates_sp.yaw = rates_setpoint(2); _rates_sp.timestamp = hrt_absolute_time(); @@ -630,7 +612,7 @@ void FixedwingAttitudeControl::Run() // _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration); const Vector3f angular_accel{v_angular_acceleration.xyz}; - const Vector3f rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + 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); _actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(att_control(0))) ? att_control( @@ -646,14 +628,14 @@ void FixedwingAttitudeControl::Run() rate_ctrl_status_s rate_ctrl_status{}; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); - rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); + // rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); + // rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); if (wheel_control) { rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); } else { - rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); + // rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); } _rate_ctrl_status_pub.publish(rate_ctrl_status); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 0ba56ca0f9..f6183fb523 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -34,12 +34,10 @@ #pragma once #include +#include #include -#include "ecl_pitch_controller.h" -#include "ecl_roll_controller.h" #include "ecl_wheel_controller.h" -#include "ecl_yaw_controller.h" #include #include #include @@ -234,11 +232,9 @@ private: (ParamFloat) _param_trim_yaw ) - ECL_RollController _roll_ctrl; - ECL_PitchController _pitch_ctrl; - ECL_YawController _yaw_ctrl; ECL_WheelController _wheel_ctrl; RateControl _rate_control; ///< class for rate control calculations + AttitudeControl _attitude_control; /**< class for attitude control calculations */ /** * @brief Update flap control setting diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp deleted file mode 100644 index d1a43a9412..0000000000 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_pitch_controller.cpp - * Implementation of a simple orthogonal pitch PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_pitch_controller.h" -#include -#include -#include - -float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && - PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.airspeed))) { - - return _rate_setpoint; - } - - /* Calculate the error */ - 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; - - 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 deleted file mode 100644 index 3ecc468a29..0000000000 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_pitch_controller.h - * Definition of a simple orthogonal pitch PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#ifndef ECL_PITCH_CONTROLLER_H -#define ECL_PITCH_CONTROLLER_H - -#include - -#include "ecl_controller.h" - -class ECL_PitchController : - public ECL_Controller -{ -public: - ECL_PitchController() = default; - ~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) - { - _max_rate = max_rate_pos; - } - - void set_max_rate_neg(float max_rate_neg) - { - _max_rate_neg = max_rate_neg; - } - - void set_bodyrate_setpoint(float rate) - { - _bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate); - } - -protected: - float _max_rate_neg{0.0f}; -}; - -#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp deleted file mode 100644 index 8d20a72c72..0000000000 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_roll_controller.cpp - * Implementation of a simple orthogonal roll PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_roll_controller.h" -#include -#include -#include - -float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && - PX4_ISFINITE(ctl_data.roll))) { - - return _rate_setpoint; - } - - /* Calculate the error */ - 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; - - 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 deleted file mode 100644 index e7ea5d238b..0000000000 --- a/src/modules/fw_att_control/ecl_roll_controller.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_roll_controller.h - * Definition of a simple orthogonal roll PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#ifndef ECL_ROLL_CONTROLLER_H -#define ECL_ROLL_CONTROLLER_H - -#include "ecl_controller.h" - -class ECL_RollController : - public ECL_Controller -{ -public: - ECL_RollController() = default; - ~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_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp deleted file mode 100644 index 09716d84e9..0000000000 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_yaw_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_yaw_controller.h" -#include -#include -#include - -float ECL_YawController::control_attitude(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.roll_rate_setpoint) && - PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { - - return _rate_setpoint; - } - - float constrained_roll; - bool inverted = false; - - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(ctl_data.roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - inverted = true; - - // inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity - //note: the ranges are extended by 10 deg here to avoid numeric resolution effects - if (ctl_data.roll > 0.0f) { - /* right hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f)); - } - } - - constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint)); - - - 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); - } - - if (!PX4_ISFINITE(_rate_setpoint)) { - PX4_WARN("yaw rate sepoint not finite"); - _rate_setpoint = 0.0f; - } - - 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 deleted file mode 100644 index d197fbcdf3..0000000000 --- a/src/modules/fw_att_control/ecl_yaw_controller.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_yaw_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#ifndef ECL_YAW_CONTROLLER_H -#define ECL_YAW_CONTROLLER_H - -#include "ecl_controller.h" - -class ECL_YawController : - public ECL_Controller -{ -public: - ECL_YawController() = default; - ~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}; - -}; - -#endif // ECL_YAW_CONTROLLER_H