mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mc_att_control: move rate control to RateControl class
This makes the controller more modular, more readable and hence better maintainable.
This commit is contained in:
Matthias Grob
parent
97e1edc835
commit
9f639d1f3b
@ -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;
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
44
src/modules/mc_att_control/RateControl/CMakeLists.txt
Normal file
44
src/modules/mc_att_control/RateControl/CMakeLists.txt
Normal file
@ -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)
|
||||
155
src/modules/mc_att_control/RateControl/RateControl.cpp
Normal file
155
src/modules/mc_att_control/RateControl/RateControl.cpp
Normal file
@ -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 <RateControl.hpp>
|
||||
#include <px4_defines.h>
|
||||
|
||||
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);
|
||||
}
|
||||
149
src/modules/mc_att_control/RateControl/RateControl.hpp
Normal file
149
src/modules/mc_att_control/RateControl/RateControl.hpp
Normal file
@ -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 <matrix/matrix/math.hpp>
|
||||
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
|
||||
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
|
||||
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;
|
||||
};
|
||||
44
src/modules/mc_att_control/RateControl/RateControlTest.cpp
Normal file
44
src/modules/mc_att_control/RateControl/RateControlTest.cpp
Normal file
@ -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 <gtest/gtest.h>
|
||||
#include <RateControl.hpp>
|
||||
|
||||
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());
|
||||
}
|
||||
@ -32,7 +32,6 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_config.h>
|
||||
@ -62,6 +61,7 @@
|
||||
#include <vtol_att_control/vtol_type.h>
|
||||
|
||||
#include <AttitudeControl.hpp>
|
||||
#include <RateControl.hpp>
|
||||
|
||||
/**
|
||||
* 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] */
|
||||
|
||||
|
||||
@ -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 <mathlib/math/Limits.hpp>
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user