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

mc_att_control: use SuperExpo input curve for acro mode to further enhance the stick feel

Browse Source
This commit is contained in:
Matthias Grob
2017-10-17 16:54:16 +02:00
parent 455ba0e1b5
commit 8b09eaf124
2 changed files with 25 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+8 -4
View File
@@ -219,6 +219,7 @@ private:
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t acro_expo;
param_t acro_superexpo;
param_t rattitude_thres;
param_t vtol_type;
@@ -258,7 +259,8 @@ private:
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */
matrix::Vector3f acro_rate_max; /**< max attitude rates in acro mode */
float acro_expo;
float acro_expo; /**< function parameter for expo stick curve shape */
float acro_superexpo; /**< function parameter for superexpo stick curve shape */
float rattitude_thres;
int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
bool vtol_opt_recovery_enabled;
@@ -495,6 +497,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.acro_expo = param_find("MC_ACRO_EXPO");
_params_handles.acro_superexpo = param_find("MC_ACRO_SUPEXPO");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.roll_tc = param_find("MC_ROLL_TC");
@@ -644,6 +647,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
param_get(_params_handles.acro_expo, &_params.acro_expo);
param_get(_params_handles.acro_superexpo, &_params.acro_superexpo);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
@@ -1246,9 +1250,9 @@ MulticopterAttitudeControl::task_main()
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
matrix::Vector3f man_rate_sp;
man_rate_sp(0) = math::expo(_manual_control_sp.y, _params.acro_expo);
man_rate_sp(1) = math::expo(-_manual_control_sp.x, _params.acro_expo);
man_rate_sp(2) = math::expo(_manual_control_sp.r, _params.acro_expo);
man_rate_sp(0) = math::superexpo(_manual_control_sp.y, _params.acro_expo, _params.acro_superexpo);
man_rate_sp(1) = math::superexpo(-_manual_control_sp.x, _params.acro_expo, _params.acro_superexpo);
man_rate_sp(2) = math::superexpo(_manual_control_sp.r, _params.acro_expo, _params.acro_superexpo);
man_rate_sp = man_rate_sp.emult(_params.acro_rate_max);
_rates_sp = math::Vector<3>(man_rate_sp.data());
_thrust_sp = _manual_control_sp.z;