From 9f639d1f3be72b2e265e6376c33100bfdbee80e9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 13 Oct 2019 14:42:20 +0200 Subject: [PATCH] mc_att_control: move rate control to RateControl class This makes the controller more modular, more readable and hence better maintainable. --- .../AttitudeControl/AttitudeControl.hpp | 18 +- src/modules/mc_att_control/CMakeLists.txt | 2 + .../mc_att_control/RateControl/CMakeLists.txt | 44 +++++ .../RateControl/RateControl.cpp | 155 ++++++++++++++++++ .../RateControl/RateControl.hpp | 149 +++++++++++++++++ .../RateControl/RateControlTest.cpp | 44 +++++ src/modules/mc_att_control/mc_att_control.hpp | 16 +- .../mc_att_control/mc_att_control_main.cpp | 133 +++------------ 8 files changed, 426 insertions(+), 135 deletions(-) create mode 100644 src/modules/mc_att_control/RateControl/CMakeLists.txt create mode 100644 src/modules/mc_att_control/RateControl/RateControl.cpp create mode 100644 src/modules/mc_att_control/RateControl/RateControl.hpp create mode 100644 src/modules/mc_att_control/RateControl/RateControlTest.cpp diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index f681120871..121f559864 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -56,15 +56,6 @@ public: AttitudeControl() = default; ~AttitudeControl() = default; - /** - * Run one control loop cycle calculation with either new - * @param q estimation of the current vehicle attitude unit quaternion - * @param qd desired vehicle attitude setpoint - * @param yawspeed_feedforward [rad/s] yaw feed forward angular rate in world frame - * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller - */ - matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward); - /** * Set proportional attitude control gain * @param proportional_gain 3D vector containing gains for roll, pitch, yaw @@ -77,6 +68,15 @@ public: */ void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + /** + * Run one control loop cycle calculation + * @param q estimation of the current vehicle attitude unit quaternion + * @param qd desired vehicle attitude setpoint + * @param yawspeed_feedforward [rad/s] yaw feed forward angular rate in world frame + * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller + */ + matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward); + private: matrix::Vector3f _proportional_gain; matrix::Vector3f _rate_limit; diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 82b8ad4e87..ffd12a2450 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(AttitudeControl) +add_subdirectory(RateControl) px4_add_module( MODULE modules__mc_att_control @@ -45,5 +46,6 @@ px4_add_module( conversion mathlib AttitudeControl + RateControl px4_work_queue ) diff --git a/src/modules/mc_att_control/RateControl/CMakeLists.txt b/src/modules/mc_att_control/RateControl/CMakeLists.txt new file mode 100644 index 0000000000..1cd08cb5b3 --- /dev/null +++ b/src/modules/mc_att_control/RateControl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 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 +# 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 PX4 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. +# +############################################################################ + +px4_add_library(RateControl + RateControl.cpp +) +target_include_directories(RateControl + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(RateControl PRIVATE mathlib) + +px4_add_unit_gtest(SRC RateControlTest.cpp LINKLIBS RateControl) diff --git a/src/modules/mc_att_control/RateControl/RateControl.cpp b/src/modules/mc_att_control/RateControl/RateControl.cpp new file mode 100644 index 0000000000..f9815894ae --- /dev/null +++ b/src/modules/mc_att_control/RateControl/RateControl.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 + * 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 PX4 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 RateControl.cpp + */ + +#include +#include + +using namespace matrix; + +void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_p = P; + _gain_i = I; + _gain_d = D; +} + +void RateControl::setDTermCutoff(const float loop_rate, const float cutoff, const bool force) +{ + // only do expensive filter update if the cutoff changed + if (force || fabsf(_lp_filters_d.get_cutoff_freq() - cutoff) > 0.01f) { + _lp_filters_d.set_cutoff_frequency(loop_rate, cutoff); + _lp_filters_d.reset(_rate_prev); + } +} + +void RateControl::setTPA(const Vector3f &breakpoint, const Vector3f &rate) +{ + _tpa_breakpoint = breakpoint; + _tpa_rate = rate; +} + +void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status) +{ + _mixer_saturation_positive[0] = status.flags.roll_pos; + _mixer_saturation_positive[1] = status.flags.pitch_pos; + _mixer_saturation_positive[2] = status.flags.yaw_pos; + _mixer_saturation_negative[0] = status.flags.roll_neg; + _mixer_saturation_negative[1] = status.flags.pitch_neg; + _mixer_saturation_negative[2] = status.flags.yaw_neg; +} + +Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const float dt, const bool landed, + const float thrust_sp) +{ + Vector3f gain_p_tpa = _gain_p.emult(tpa_attenuations(_tpa_breakpoint(0), _tpa_rate(0), thrust_sp)); + Vector3f gain_i_tpa = _gain_i.emult(tpa_attenuations(_tpa_breakpoint(1), _tpa_rate(1), thrust_sp)); + Vector3f gain_d_tpa = _gain_d.emult(tpa_attenuations(_tpa_breakpoint(2), _tpa_rate(2), thrust_sp)); + + // angular rates error + Vector3f rate_error = rate_sp - rate; + + // prepare D-term based on low-pass filtered rates + Vector3f rate_filtered(_lp_filters_d.apply(rate)); + Vector3f rate_d; + + if (dt > FLT_EPSILON) { + rate_d = (rate_filtered - _rate_prev_filtered) / dt; + } + + // PID control with feed forward + Vector3f torque = gain_p_tpa.emult(rate_error) + _rate_int - gain_d_tpa.emult(rate_d) + _gain_ff.emult(rate_sp); + + _rate_prev = rate; + _rate_prev_filtered = rate_filtered; + + // update integral only if we are not landed + if (!landed) { + updateIntegral(rate_error, dt, gain_i_tpa); + } + + return torque; +} + +void RateControl::updateIntegral(Vector3f &rate_error, const float dt, const Vector3f &gain_i_tpa) +{ + for (int i = 0; i < 3; i++) { + // prevent further positive control saturation + if (_mixer_saturation_positive[i]) { + rate_error(i) = math::min(rate_error(i), 0.f); + } + + // prevent further negative control saturation + if (_mixer_saturation_negative[i]) { + rate_error(i) = math::max(rate_error(i), 0.f); + } + + // I term factor: reduce the I gain with increasing rate error. + // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint + // change (noticeable in a bounce-back effect after a flip). + // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: + // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), + // and up to 200 deg error leads to <25% reduction of I. + float i_factor = rate_error(i) / math::radians(400.f); + i_factor = math::max(0.0f, 1.f - i_factor * i_factor); + + // Perform the integration using a first order method + float rate_i = _rate_int(i) + i_factor * gain_i_tpa(i) * rate_error(i) * dt; + + // do not propagate the result if out of range or invalid + if (PX4_ISFINITE(rate_i)) { + _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); + } + } +} + +void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) +{ + rate_ctrl_status.rollspeed_integ = _rate_int(0); + rate_ctrl_status.pitchspeed_integ = _rate_int(1); + rate_ctrl_status.yawspeed_integ = _rate_int(2); +} + +Vector3f RateControl::tpa_attenuations(float tpa_breakpoint, float tpa_rate, float thrust_sp) +{ + static constexpr float tpa_rate_lower_limit = 0.05f; + + /* throttle pid attenuation factor */ + float tpa = 1.0f - tpa_rate * (fabsf(thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint); + tpa = fmaxf(tpa_rate_lower_limit, fminf(1.0f, tpa)); + + return Vector3f(tpa, tpa, 1.f); +} diff --git a/src/modules/mc_att_control/RateControl/RateControl.hpp b/src/modules/mc_att_control/RateControl/RateControl.hpp new file mode 100644 index 0000000000..7acbd68f12 --- /dev/null +++ b/src/modules/mc_att_control/RateControl/RateControl.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 + * 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 PX4 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 RateControl.hpp + * + * PID 3 axis angular rate / angular velocity control. + */ + +#pragma once + +#include +#include + +#include +#include + +class RateControl +{ +public: + RateControl() = default; + ~RateControl() = default; + + /** + * Set the rate control gains + * @param P 3D vector of proportional gains for body x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the mximum absolute value of the integrator for all axes + * @param integrator_limit limit value for all axes x, y, z + */ + void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; }; + + /** + * Set update frequency and low-pass filter cutoff that is applied to the derivative term + * @param loop_rate [Hz] rate with which update function is called + * @param cutoff [Hz] cutoff frequency for the low-pass filter on the dervative term + * @param force flag to force an expensive update even if the cutoff didn't change + */ + void setDTermCutoff(const float loop_rate, const float cutoff, const bool force); + + /** + * Set direct rate to torque feed forward gain + * @see _gain_ff + * @param FF 3D vector of feed forward gains for body x,y,z axis + */ + void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; + + /** + * Set the rate control gains + * @param breakpoint parameter 3D vector for P, I and D + * @param rate parameter 3D vector for P, I and D + */ + void setTPA(const matrix::Vector3f &breakpoint, const matrix::Vector3f &rate); + + /** + * Set saturation status + * @param status message from mixer reporting about saturation + */ + void setSaturationStatus(const MultirotorMixer::saturation_status &status); + + /** + * Run one control loop cycle calculation + * @param rate estimation of the current vehicle angular rate + * @param rate_sp desired vehicle angular rate setpoint + * @param dt desired vehicle angular rate setpoint + * @param thrust_sp total thrust setpoint to be used for TPA + * @return [-1,1] normalized torque vector to apply to the vehicle + */ + matrix::Vector3f update(const matrix::Vector3f rate, const matrix::Vector3f rate_sp, const float dt, const bool landed, + const float thrust_sp); + + /** + * Set the integral term to 0 to prevent windup + * @see _rate_int + */ + void resetIntegral() { _rate_int.zero(); } + + /** + * Get status message of controller for logging/debugging + * @param rate_ctrl_status status message to fill with internal states + */ + void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + +private: + void updateIntegral(matrix::Vector3f &rate_error, const float dt, const matrix::Vector3f &gain_i_tpa); + + // Gains + matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z + matrix::Vector3f _gain_i; ///< rate control integral gain + matrix::Vector3f _gain_d; ///< rate control derivative gain + matrix::Vector3f _lim_int; ///< integrator term maximum absolute value + matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters + + // States + matrix::Vector3f _rate_prev; ///< angular rates of previous update + matrix::Vector3f _rate_prev_filtered; ///< low-pass filtered angular rates of previous update + matrix::Vector3f _rate_int; ///< integral term of the rate controller + math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw) + bool _mixer_saturation_positive[3] {}; + bool _mixer_saturation_negative[3] {}; + + /* + * Throttle PID attenuation + * Lowers the overall gain of the PID controller linearly depending on total thrust. + * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje + * @param tpa_breakpoint + * @param tpa_rate + * @param thrust_sp + * @return attenuation [0,1] per axis in a vector + */ + matrix::Vector3f tpa_attenuations(float tpa_breakpoint, float tpa_rate, float thrust_sp); + matrix::Vector3f _tpa_breakpoint; + matrix::Vector3f _tpa_rate; +}; diff --git a/src/modules/mc_att_control/RateControl/RateControlTest.cpp b/src/modules/mc_att_control/RateControl/RateControlTest.cpp new file mode 100644 index 0000000000..bab1848cfd --- /dev/null +++ b/src/modules/mc_att_control/RateControl/RateControlTest.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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 + * 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 PX4 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. + * + ****************************************************************************/ + +#include +#include + +using namespace matrix; + +TEST(RateControlTest, AllZeroCase) +{ + RateControl rate_control; + Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false, 0.f); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 10fb522d63..ca7396e32b 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include -#include #include #include #include @@ -62,6 +61,7 @@ #include #include +#include /** * Multicopter attitude control app start / stop handling function @@ -140,7 +140,8 @@ private: */ matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate); - AttitudeControl _attitude_control; /**< class for attitude control calculations */ + AttitudeControl _attitude_control; ///< class for attitude control calculations + RateControl _rate_control; ///< class for rate control calculations uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ @@ -183,14 +184,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::LowPassFilter2pVector3f _lp_filters_d{initial_update_rate_hz, 50.f}; /**< low-pass filters for D-term (roll, pitch & yaw) */ static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ - matrix::Vector3f _rates_prev; /**< angular rates on previous step */ - matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - matrix::Vector3f _rates_int; /**< angular rates integral error */ matrix::Vector3f _att_control; /**< attitude control vector */ float _thrust_sp{0.0f}; /**< thrust setpoint */ @@ -273,13 +270,6 @@ private: bool _is_tailsitter{false}; - matrix::Vector3f _rate_p; /**< P gain for angular rate error */ - matrix::Vector3f _rate_i; /**< I gain for angular rate error */ - matrix::Vector3f _rate_int_lim; /**< integrator state limit for rate loop */ - matrix::Vector3f _rate_d; /**< D gain for angular rate error */ - matrix::Vector3f _rate_ff; /**< Feedforward gain for desired rates */ - matrix::Vector3f _rate_k; /**< Rate controller global gain */ - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 87786dd089..cf0b6fff56 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 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 @@ -52,13 +52,6 @@ #include #include -#define TPA_RATE_LOWER_LIMIT 0.05f - -#define AXIS_INDEX_ROLL 0 -#define AXIS_INDEX_PITCH 1 -#define AXIS_INDEX_YAW 2 -#define AXIS_COUNT 3 - using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl() : @@ -72,10 +65,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_att.q[0] = 1.f; _v_att_sp.q_d[0] = 1.f; - _rates_prev.zero(); - _rates_prev_filtered.zero(); _rates_sp.zero(); - _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); @@ -104,21 +94,19 @@ MulticopterAttitudeControl::parameters_updated() // Store some of the parameters in a more convenient way & precompute often-used values _attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get())); - // rate gains - _rate_p = Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get()); - _rate_i = Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get()); - _rate_int_lim = Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()); - _rate_d = Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()); - _rate_ff = Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()); - + // rate control parameters // The controller gain K is used to convert the parallel (P + I/s + sD) form // to the ideal (K * [1 + 1/sTi + sTd]) form - _rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); - - if (fabsf(_lp_filters_d.get_cutoff_freq() - _param_mc_dterm_cutoff.get()) > 0.01f) { - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); - _lp_filters_d.reset(_rates_prev); - } + Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + _rate_control.setGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false); + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); // angular rate limits using math::radians; @@ -373,27 +361,6 @@ MulticopterAttitudeControl::control_attitude() _rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate); } -/* - * Throttle PID attenuation - * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje - * Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp' - * Output: 'pidAttenuationPerAxis' vector - */ -Vector3f -MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) -{ - /* throttle pid attenuation factor */ - float tpa = 1.0f - tpa_rate * (fabsf(_thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint); - tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); - - Vector3f pidAttenuationPerAxis; - pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa; - pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa; - pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0; - - return pidAttenuationPerAxis; -} - /* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' @@ -402,71 +369,14 @@ MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rat void MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates) { - /* reset integral if disarmed */ + // reset integral if disarmed if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rates_int.zero(); + _rate_control.resetIntegral(); } - Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_param_mc_tpa_break_p.get(), _param_mc_tpa_rate_p.get())); - Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_param_mc_tpa_break_i.get(), _param_mc_tpa_rate_i.get())); - Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_param_mc_tpa_break_d.get(), _param_mc_tpa_rate_d.get())); - - /* angular rates error */ - Vector3f rates_err = _rates_sp - rates; - - /* apply low-pass filtering to the rates for D-term */ - Vector3f rates_filtered(_lp_filters_d.apply(rates)); - - _att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) - - rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) + - _rates_int + - _rate_ff.emult(_rates_sp); - - _rates_prev = rates; - _rates_prev_filtered = rates_filtered; - - /* update integral only if we are not landed */ - if (!_vehicle_land_detected.maybe_landed && !_vehicle_land_detected.landed) { - for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { - // Check for positive control saturation - bool positive_saturation = - ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) || - ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) || - ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos); - - // Check for negative control saturation - bool negative_saturation = - ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) || - ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) || - ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg); - - // prevent further positive control saturation - if (positive_saturation) { - rates_err(i) = math::min(rates_err(i), 0.0f); - } - - // prevent further negative control saturation - if (negative_saturation) { - rates_err(i) = math::max(rates_err(i), 0.0f); - } - - // I term factor: reduce the I gain with increasing rate error. - // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint - // change (noticeable in a bounce-back effect after a flip). - // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: - // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), - // and up to 200 deg error leads to <25% reduction of I. - float i_factor = rates_err(i) / math::radians(400.f); - i_factor = math::max(0.0f, 1.f - i_factor * i_factor); - - // Perform the integration using a first order method and do not propagate the result if out of range or invalid - float rate_i = _rates_int(i) + i_factor * _rate_k(i) * rates_i_scaled(i) * rates_err(i) * dt; - - if (PX4_ISFINITE(rate_i)) { - _rates_int(i) = math::constrain(rate_i, -_rate_int_lim(i), _rate_int_lim(i)); - } - } - } + const bool landed = _vehicle_land_detected.maybe_landed || _vehicle_land_detected.landed; + _rate_control.setSaturationStatus(_saturation_status); + _att_control = _rate_control.update(rates, _rates_sp, dt, landed, _thrust_sp); } void @@ -488,10 +398,7 @@ MulticopterAttitudeControl::publish_rate_controller_status() { rate_ctrl_status_s rate_ctrl_status = {}; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed_integ = _rates_int(0); - rate_ctrl_status.pitchspeed_integ = _rates_int(1); - rate_ctrl_status.yawspeed_integ = _rates_int(2); - + _rate_control.getRateControlStatus(rate_ctrl_status); _controller_status_pub.publish(rate_ctrl_status); } @@ -634,7 +541,7 @@ MulticopterAttitudeControl::Run() if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { _rates_sp.zero(); - _rates_int.zero(); + _rate_control.resetIntegral(); _thrust_sp = 0.0f; _att_control.zero(); publish_actuator_controls(); @@ -661,7 +568,7 @@ MulticopterAttitudeControl::Run() _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; _dt_accumulator = 0; _loop_counter = 0; - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); + _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true); } }