From 7e8cf87d0d08f30fb4bf181731a37b65bcc6d0e5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Feb 2019 17:03:14 +0000 Subject: [PATCH] mc_att_control: move attitude control calculations into separate class to modularize and simplify unit testing --- .../AttitudeControl/AttitudeControl.cpp | 111 ++++++++++++++++++ .../AttitudeControl/AttitudeControl.hpp | 84 +++++++++++++ .../AttitudeControl/CMakeLists.txt | 40 +++++++ src/modules/mc_att_control/CMakeLists.txt | 3 + src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 75 ++---------- 6 files changed, 248 insertions(+), 69 deletions(-) create mode 100644 src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp create mode 100644 src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp create mode 100644 src/modules/mc_att_control/AttitudeControl/CMakeLists.txt diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 0000000000..6284fbb431 --- /dev/null +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 AttitudeControl.cpp + */ + +#include + +#include +#include + +using namespace matrix; + +void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain) +{ + _proportional_gain = proportional_gain; + + // prepare yaw weight from the ratio between roll/pitch and yaw gains + const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.f; + _yaw_w = math::constrain(proportional_gain(2) / roll_pitch_gain, 0.f, 1.f); + + _proportional_gain(2) = roll_pitch_gain; +} + +matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward) +{ + // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN + q.normalize(); + qd.normalize(); + + // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch + Vector3f e_z = q.dcm_z(); + Vector3f e_z_d = qd.dcm_z(); + Quatf qd_red(e_z, e_z_d); + + if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) { + // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, + // full attitude control anyways generates no yaw input and directly takes the combination of + // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. + qd_red = qd; + + } else { + // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + qd_red *= q; + } + + // mix full and reduced desired attitude + Quatf q_mix = qd_red.inversed() * qd; + q_mix *= math::signNoZero(q_mix(0)); + // catch numerical problems with the domain of acosf and asinf + q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); + q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); + qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + + // quaternion attitude control law, qe is rotation from q to qd + Quatf qe = q.inversed() * qd; + + // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + // also taking care of the antipodal unit quaternion ambiguity + Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag(); + + // calculate angular rates setpoint + matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); + + // Feed forward the yaw setpoint rate. + // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis, + // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). + // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) + // and multiply it by the yaw setpoint rate (yaw_sp_move_rate). + // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame + // such that it can be added to the rates setpoint. + rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward; + + // limit rates + for (int i = 0; i < 3; i++) { + rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); + } + + return rate_setpoint; +} diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 0000000000..f681120871 --- /dev/null +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 AttitudeControl.hpp + * + * A quaternion based attitude controller. + * + * @author Matthias Grob + * + * Publication documenting the implemented Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich + * + * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + */ + +#pragma once + +#include + +class AttitudeControl +{ +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 + */ + void setProportionalGain(const matrix::Vector3f &proportional_gain); + + /** + * Set hard limit for output rate setpoints + * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw + */ + void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + +private: + matrix::Vector3f _proportional_gain; + matrix::Vector3f _rate_limit; + float _yaw_w = 0.0f; /**< yaw weight [0,1] to prioritize roll and pitch */ +}; diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..d0631f71c1 --- /dev/null +++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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(AttitudeControl + AttitudeControl.cpp +) +target_include_directories(AttitudeControl +PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 588e43f786..3734e79b29 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +add_subdirectory(AttitudeControl) + px4_add_module( MODULE modules__mc_att_control MAIN mc_att_control @@ -43,4 +45,5 @@ px4_add_module( circuit_breaker conversion mathlib + AttitudeControl ) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index c5c660c85d..8d53a55c67 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -58,6 +58,8 @@ #include #include +#include + /** * Multicopter attitude control app start / stop handling function */ @@ -145,6 +147,7 @@ private: */ matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate); + AttitudeControl _attitude_control; /**< class for attitude control calculations */ int _v_att_sub{-1}; /**< vehicle attitude subscription */ int _v_att_sp_sub{-1}; /**< vehicle attitude setpoint subscription */ @@ -274,7 +277,6 @@ private: (ParamInt) _airmode ) - matrix::Vector3f _attitude_p; /**< P gain for attitude control */ 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 */ 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 06c6971ba3..6880681d84 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -133,8 +133,9 @@ MulticopterAttitudeControl::parameters_updated() { /* Store some of the parameters in a more convenient way & precompute often-used values */ + _attitude_control.setProportionalGain(Vector3f(_roll_p.get(), _pitch_p.get(), _yaw_p.get())); + /* roll gains */ - _attitude_p(0) = _roll_p.get(); _rate_p(0) = _roll_rate_p.get(); _rate_i(0) = _roll_rate_i.get(); _rate_int_lim(0) = _roll_rate_integ_lim.get(); @@ -142,7 +143,6 @@ MulticopterAttitudeControl::parameters_updated() _rate_ff(0) = _roll_rate_ff.get(); /* pitch gains */ - _attitude_p(1) = _pitch_p.get(); _rate_p(1) = _pitch_rate_p.get(); _rate_i(1) = _pitch_rate_i.get(); _rate_int_lim(1) = _pitch_rate_integ_lim.get(); @@ -150,7 +150,6 @@ MulticopterAttitudeControl::parameters_updated() _rate_ff(1) = _pitch_rate_ff.get(); /* yaw gains */ - _attitude_p(2) = _yaw_p.get(); _rate_p(2) = _yaw_rate_p.get(); _rate_i(2) = _yaw_rate_i.get(); _rate_int_lim(2) = _yaw_rate_integ_lim.get(); @@ -542,75 +541,15 @@ MulticopterAttitudeControl::control_attitude() // physical thrust axis is the negative of body z axis _thrust_sp = -_v_att_sp.thrust_body[2]; - /* prepare yaw weight from the ratio between roll/pitch and yaw gains */ - Vector3f attitude_gain = _attitude_p; - const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f; - const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f); - attitude_gain(2) = roll_pitch_gain; - - /* get estimated and desired vehicle attitude */ - Quatf q(_v_att.q); - Quatf qd(_v_att_sp.q_d); - - /* ensure input quaternions are exactly normalized because acosf(1.00001) == NaN */ - q.normalize(); - qd.normalize(); - - /* calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch */ - Vector3f e_z = q.dcm_z(); - Vector3f e_z_d = qd.dcm_z(); - Quatf qd_red(e_z, e_z_d); - - if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) { - /* In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, - * full attitude control anyways generates no yaw input and directly takes the combination of - * roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. */ - qd_red = qd; + if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && + !_v_control_mode.flag_control_manual_enabled) { + _attitude_control.setRateLimit(_auto_rate_max); } else { - /* transform rotation from current to desired thrust vector into a world frame reduced desired attitude */ - qd_red *= q; + _attitude_control.setRateLimit(_mc_rate_max); } - /* mix full and reduced desired attitude */ - Quatf q_mix = qd_red.inversed() * qd; - q_mix *= math::signNoZero(q_mix(0)); - /* catch numerical problems with the domain of acosf and asinf */ - q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); - q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); - qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3)))); - - /* quaternion attitude control law, qe is rotation from q to qd */ - Quatf qe = q.inversed() * qd; - - /* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) - * also taking care of the antipodal unit quaternion ambiguity */ - Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag(); - - /* calculate angular rates setpoint */ - _rates_sp = eq.emult(attitude_gain); - - /* Feed forward the yaw setpoint rate. - * yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis, - * but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). - * Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) - * and multiply it by the yaw setpoint rate (yaw_sp_move_rate). - * This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame - * such that it can be added to the rates setpoint. - */ - _rates_sp += q.inversed().dcm_z() * _v_att_sp.yaw_sp_move_rate; - - - /* limit rates */ - for (int i = 0; i < 3; i++) { - if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && - !_v_control_mode.flag_control_manual_enabled) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_auto_rate_max(i), _auto_rate_max(i)); - - } else { - _rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i)); - } - } + _rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate); } /*