dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Matthias Grob 2017-10-03 09:01:47 +02:00
parent 455ba0e1b5
commit 8b09eaf124
2 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mc_att_control/mc_att_control_main.cpp
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;

18
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -406,7 +406,8 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 700.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 700.0f);
/**
* Acro expo factor
* Acro Expo factor
* applied to input of all axis: roll, pitch, yaw
*
* 0 Purely linear input curve
* 1 Purely cubic input curve
@ -418,6 +419,21 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 700.0f);
*/
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
/**
* Acro SuperExpo factor
* applied to input of all axis: roll, pitch, yaw
*
* 0 Pure Expo function
* 0.7 resonable shape enhancement for intuitive stick feel
* 0.95 very strong bent input curve only near maxima have effect
*
* @min 0
* @max 0.95
* @decimal 2
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f);
/**
* Threshold for Rattitude mode
*