fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 16:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete unused tailsitter_recovery library (#9103)

Browse Source
This commit is contained in:
Daniel Agar
2018-03-21 14:09:36 -04:00
committed by GitHub
parent b7bfeb442e
commit ac7242987c
7 changed files with 2 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/CMakeLists.txt
-4
View File
@@ -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
)
src/modules/mc_att_control/mc_att_control_main.cpp
+1 -43
View File
@@ -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 <conversion/rotation.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
@@ -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);
src/modules/mc_att_control/tailsitter_recovery/CMakeLists.txt
-36
View File
@@ -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
)
src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.cpp
-183
View File
@@ -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 <math.h>
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;
}
src/modules/mc_att_control/tailsitter_recovery/tailsitter_recovery.h
-77
View File
@@ -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 <bapstroman@gmail.com>
*/
#pragma once
#include <lib/mathlib/mathlib.h>
#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
};
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1 -11
View File
@@ -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
src/modules/vtol_att_control/vtol_att_control_params.c
-8
View File
@@ -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
*