From ac7242987c7e546c02e857258670560b836a25c7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Mar 2018 14:09:36 -0400 Subject: [PATCH] delete unused tailsitter_recovery library (#9103) --- src/modules/mc_att_control/CMakeLists.txt | 4 - .../mc_att_control/mc_att_control_main.cpp | 44 +---- .../tailsitter_recovery/CMakeLists.txt | 36 ---- .../tailsitter_recovery.cpp | 183 ------------------ .../tailsitter_recovery/tailsitter_recovery.h | 77 -------- .../mc_pos_control/mc_pos_control_main.cpp | 12 +- .../vtol_att_control_params.c | 8 - 7 files changed, 2 insertions(+), 362 deletions(-) delete mode 100644 src/modules/mc_att_control/tailsitter_recovery/CMakeLists.txt delete mode 100644 src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.cpp delete mode 100644 src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.h diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index b6a3af0ca5..9df8e86732 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################ -add_subdirectory(tailsitter_recovery) - px4_add_module( MODULE modules__mc_att_control MAIN mc_att_control @@ -41,6 +39,4 @@ px4_add_module( COMPILE_FLAGS SRCS mc_att_control_main.cpp - DEPENDS - tailsitter_recovery ) 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 7d1d4acb71..8371947b1e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -55,8 +55,6 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include "tailsitter_recovery/tailsitter_recovery.h" - #include #include #include @@ -223,8 +221,6 @@ private: param_t acro_superexpo; param_t rattitude_thres; - param_t vtol_type; - param_t vtol_opt_recovery_enabled; param_t vtol_wv_yaw_rate_scale; param_t bat_scale_en; @@ -264,8 +260,6 @@ private: float acro_superexpo; /**< function parameter for superexpo stick curve shape */ float rattitude_thres; - int32_t vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ - bool vtol_opt_recovery_enabled; float vtol_wv_yaw_rate_scale; /**< Scale value [0, 1] for yaw rate setpoint */ int32_t bat_scale_en; @@ -276,8 +270,6 @@ private: } _params; - TailsitterRecovery *_ts_opt_recovery{nullptr}; /**< Computes optimal rates for tailsitter recovery */ - /** * Update our local parameter cache. */ @@ -511,8 +503,6 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - delete _ts_opt_recovery; - mc_att_control::g_control = nullptr; } @@ -613,16 +603,9 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); if (_vehicle_status.is_vtol) { - param_get(_params_handles.vtol_type, &_params.vtol_type); - - int32_t tmp; - param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); - _params.vtol_opt_recovery_enabled = (tmp == 1); - param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale); } else { - _params.vtol_opt_recovery_enabled = false; _params.vtol_wv_yaw_rate_scale = 0.f; } @@ -730,17 +713,10 @@ MulticopterAttitudeControl::vehicle_status_poll() _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); - _params_handles.vtol_type = param_find("VT_TYPE"); - _params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN"); _params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL"); parameters_update(); - if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) { - // the vehicle is a tailsitter, use optimal recovery control strategy - _ts_opt_recovery = new TailsitterRecovery(); - } - } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -1153,25 +1129,7 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_attitude_enabled) { - if (_ts_opt_recovery == nullptr) { - // the tailsitter recovery instance has not been created, thus, the vehicle - // is not a tailsitter, do normal attitude control - control_attitude(dt); - - } else { - vehicle_attitude_setpoint_poll(); - _thrust_sp = _v_att_sp.thrust; - math::Quaternion q(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]); - math::Quaternion q_sp(&_v_att_sp.q_d[0]); - math::Vector<3> attitude_p(_params.attitude_p.data()); - _ts_opt_recovery->setAttGains(attitude_p, _params.yaw_ff); - _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); - - /* limit rates */ - for (int i = 0; i < 3; i++) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); - } - } + control_attitude(dt); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); diff --git a/src/modules/mc_att_control/tailsitter_recovery/CMakeLists.txt b/src/modules/mc_att_control/tailsitter_recovery/CMakeLists.txt deleted file mode 100644 index edafafe368..0000000000 --- a/src/modules/mc_att_control/tailsitter_recovery/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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(tailsitter_recovery - tailsitter_recovery.cpp -) \ No newline at end of file diff --git a/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.cpp b/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.cpp deleted file mode 100644 index 9d1a9aa32d..0000000000 --- a/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 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 tailsitter_recovery.cpp -*/ - -#include "tailsitter_recovery.h" -#include - -TailsitterRecovery::TailsitterRecovery(): - _in_recovery_mode(false), - _yaw_ff(0.0f) -{ - _att_p(0) = _att_p(1) = _att_p(2) = 4.0f; -} - -TailsitterRecovery::~TailsitterRecovery() -{ - -} - -void TailsitterRecovery::setAttGains(math::Vector<3> &att_p, float yaw_ff) -{ - _att_p = att_p; - _yaw_ff = yaw_ff; -} - -void TailsitterRecovery::calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, - math::Vector<3> &rates_opt) -{ - math::Matrix<3, 3> R = q.to_dcm(); - - // compute error quaternion - math::Quaternion q_sp_inv = {q_sp(0), -q_sp(1), -q_sp(2), -q_sp(3)}; - math::Quaternion q_error = q * q_sp_inv; - - // compute tilt angle and corresponding tilt axis - math::Vector<3> zB = {0, 0, -1.0f}; - - math::Vector<3> zI = q_error.conjugate(zB); - - float tilt_angle; - float inner_prod = zI * zB; - - if (inner_prod >= 1.0f) { - tilt_angle = 0.0f; - - } else if (inner_prod <= -1.0f) { - tilt_angle = M_PI_F; - - } else { - tilt_angle = acosf(inner_prod); - } - - math::Vector<3> tilt_axis = {0, 0, -1}; - - if (math::min(fabsf(tilt_angle), fabsf(tilt_angle - M_PI_F)) > 0.00001f) { - tilt_axis = zI % zB; - } - - tilt_axis = R.transposed() * tilt_axis; - - // compute desired rates based on tilt angle and tilt axis - float tilt_dir = atan2f(tilt_axis(0), tilt_axis(1)); - float sign_x; - float sign_y; - float sign_z; - - if (tilt_dir < -M_PI_2_F) { - tilt_dir += M_PI_F; - sign_x = -1; - sign_y = -1; - sign_z = 1; - - } else if (tilt_dir < 0.0f) { - tilt_dir = 0.0f - tilt_dir; - sign_x = -1; - sign_y = 1; - sign_z = -1; - - } else if (tilt_dir < M_PI_2_F) { - tilt_dir += 0; - sign_x = 1; - sign_y = 1; - sign_z = 1; - - } else { - tilt_dir = M_PI_F - tilt_dir; - sign_x = 1; - sign_y = -1; - sign_z = -1; - } - - // optimal coefficients - float pwx_kx1 = -1.676f; - float pwx_ky1 = 1.38f; - float pwx_ky2 = 0.8725f; - float pwx_sx0 = 0.3586f; - float pwx_sx1 = 2.642f; - - float pwy_kx1 = -3.997f; - float pwy_sx0 = 2.133f; - float pwy_sx1 = 4.013f; - - float pwz_kx1 = 2.726f; - float pwz_ky1 = 3.168f; - float pwz_ky2 = -0.3913f; - float pwz_sx0 = 1.75f; - float pwz_sx1 = 2.298f; - - float x = tilt_angle; - float y = tilt_dir; - - rates_opt(1) = (pwx_ky1 * (M_PI_2_F - y) + pwx_ky2 * powf(M_PI_2_F - y, 2)) * - (pwx_kx1 * x * SigmoidFunction(pwx_sx1 * (pwx_sx0 - x)) - pwx_kx1 * x * expf(pwx_sx1 * - (pwx_sx0 - M_PI_F)) * SigmoidFunction(pwx_sx1 * (M_PI_F - pwx_sx0))); - rates_opt(0) = pwy_kx1 * x + powf((M_PI_2_F - y) / (M_PI_2_F), - 2) * (-pwy_kx1 * x * SigmoidFunction(pwy_sx1 * (pwy_sx0 - x))); - rates_opt(2) = (pwz_ky1 * (M_PI_2_F - y) + pwz_ky2 * powf(M_PI_2_F - y, 2)) * - (pwz_kx1 * (x - M_PI_F) * SigmoidFunction(pwz_sx1 * (x - pwz_sx0)) - pwz_kx1 * (x - M_PI_F) * SigmoidFunction( - -pwz_sx0 * pwz_sx1)); - rates_opt(0) *= -sign_x; - rates_opt(1) *= -sign_y * 1.5f; - rates_opt(2) *= -sign_z; - - float yaw_w = R(2, 2) > 0.0f ? R(2, 2) : 0.0f; - yaw_w = math::constrain(yaw_w, 0.0f, 1.0f); - - // we activate recovery mode if our tilt error is more that 30 degrees or - if (!_in_recovery_mode) { - if (fabsf(tilt_angle) > math::radians(30.0f)) { - _in_recovery_mode = true; - } - - } else { - if (fabsf(tilt_angle) < math::radians(5.0f)) { - _in_recovery_mode = false; - } - } - - // do normal attitude control, the other case has already been handled - if (!_in_recovery_mode) { - q_error = q_sp_inv * q; - rates_opt = q_error(0) > 0.0f ? _att_p.emult(q_error.imag()) * (-2.0f) : _att_p.emult(q_error.imag()) * (2.0f); - // don't want too strong yaw control. after recovery vehicle might have - // very large yaw error and shouldn't turn too fast - rates_opt(2) = math::constrain(rates_opt(2), -1.0f, 1.0f); - } - - rates_opt(2) += yaw_move_rate * yaw_w * _yaw_ff; - -} diff --git a/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.h b/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.h deleted file mode 100644 index be6e2c18fa..0000000000 --- a/src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 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 tailsitter_recovery.h - * - * Tailsitter optimal rate controller (underactuated pitch axis) - * - * This function can be used to compute desired body rates of a tailsitter - * suffering from an underactuated pitch axis. Tailsitters which produce a pitching moment - * from airflow over control surfaces mostly suffer from an underactuated pitch axis. - * This functions captures the solution of an optimal control problem which minimises - * the vehicle's tilt error and which penalises the desired rates of the underactuated - * pitch axis. - * Publication: - * Robin Ritz and Raffaello D'Andrea. A Global Strategy for Tailsitter Hover Control. - * - * @author Roman Bapst -*/ - -#pragma once - -#include - -#define SigmoidFunction(val) 1/(1 + expf(-val)) - -class TailsitterRecovery -{ -public: - TailsitterRecovery(); - ~TailsitterRecovery(); - - // Calculate the optimal rates: - // If the vehicle is not in need of a recovery, this function will do normal - // attitude control based on attitude error. If a recovery situation is detected - // then the rates are computed in an optimal way as described above. - void calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, math::Vector<3> &rates_opt); - - // Set the gains of the controller attitude loop. - void setAttGains(math::Vector<3> &att_p, float yaw_ff); - -private: - bool _in_recovery_mode; // indicates that the tailsitter is performing a recovery to hover - - math::Vector<3> _att_p; // gains for attitude loop - float _yaw_ff; // yaw feed forward gain -}; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 19721cb778..4578a8c03f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -228,7 +228,6 @@ private: param_t hold_max_xy; param_t hold_max_z; param_t alt_mode; - param_t opt_recover; param_t rc_flt_smp_rate; param_t rc_flt_cutoff; param_t acc_max_flow_xy; @@ -256,8 +255,6 @@ private: float slow_land_alt2; int32_t alt_mode; - bool opt_recover; - float rc_flt_smp_rate; float rc_flt_cutoff; float acc_max_flow_xy; @@ -669,12 +666,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.alt_mode, &v_i); _params.alt_mode = v_i; - if (_vehicle_status.is_vtol) { - int32_t i = 0; - param_get(_params_handles.opt_recover, &i); - _params.opt_recover = (i == 1); - } - /* mc attitude control parameters*/ /* manual control scale */ param_get(_params_handles.man_tilt_max, &_params.man_tilt_max); @@ -742,7 +733,6 @@ MulticopterPositionControl::poll_subscriptions() if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); - _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); parameters_update(true); } else { @@ -2951,7 +2941,7 @@ MulticopterPositionControl::generate_attitude_setpoint() _att_sp.yaw_body += euler_sp(2); /* only if we're a VTOL and optimal recovery is not used, modify roll/pitch */ - if (_vehicle_status.is_vtol && !_params.opt_recover) { + if (_vehicle_status.is_vtol) { // construct attitude setpoint rotation matrix. modify the setpoints for roll // and pitch such that they reflect the user's intention even if a yaw error // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index a9cefd6cd1..96316eff53 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -197,14 +197,6 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); */ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); -/** - * Optimal recovery strategy for pitch-weak tailsitters - * - * @boolean - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); - /** * Front transition timeout *