Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MC acro: add separate params to configure yaw expo #9358

Merged
merged 2 commits into from
Apr 26, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions src/modules/mc_att_control/mc_att_control.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -220,8 +220,10 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _acro_roll_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _acro_pitch_max,
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _acro_yaw_max,
(ParamFloat<px4::params::MC_ACRO_EXPO>) _acro_expo, /**< function parameter for expo stick curve shape */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _acro_superexpo, /**< function parameter for superexpo stick curve shape */
(ParamFloat<px4::params::MC_ACRO_EXPO>) _acro_expo_rp, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _acro_expo_y, /**< expo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _acro_superexpo_rp, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _acro_superexpo_y, /**< superexpo stick curve shape (yaw) */

(ParamFloat<px4::params::MC_RATT_TH>) _rattitude_thres,

Expand Down
6 changes: 3 additions & 3 deletions src/modules/mc_att_control/mc_att_control_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -705,9 +705,9 @@ MulticopterAttitudeControl::run()
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
Vector3f man_rate_sp(
math::superexpo(_manual_control_sp.y, _acro_expo.get(), _acro_superexpo.get()),
math::superexpo(-_manual_control_sp.x, _acro_expo.get(), _acro_superexpo.get()),
math::superexpo(_manual_control_sp.r, _acro_expo.get(), _acro_superexpo.get()));
math::superexpo(_manual_control_sp.y, _acro_expo_rp.get(), _acro_superexpo_rp.get()),
math::superexpo(-_manual_control_sp.x, _acro_expo_rp.get(), _acro_superexpo_rp.get()),
math::superexpo(_manual_control_sp.r, _acro_expo_y.get(), _acro_superexpo_y.get()));
_rates_sp = man_rate_sp.emult(_acro_rate_max);
_thrust_sp = _manual_control_sp.z;

Expand Down
39 changes: 32 additions & 7 deletions src/modules/mc_att_control/mc_att_control_params.c
Original file line number Diff line number Diff line change
Expand Up @@ -347,7 +347,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRAUTO_MAX, 45.0f);
*
* @unit deg/s
* @min 0.0
* @max 1000.0
* @max 1800.0
Copy link
Member

@MaEtUgR MaEtUgR Apr 26, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you do experiements about the overshoot not accidentally breaching the 2000deg/s gyro limit barrier? I don't have experience with >1000 deg/s rates.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, not that high, but I did go over 1000. Would be interesting to know indeed.
Looks like others are actually going to the 2000 limit: betaflight/betaflight-configurator#997

* @decimal 1
* @increment 5
* @group Multicopter Attitude Control
Expand All @@ -360,7 +360,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f);
*
* @unit deg/s
* @min 0.0
* @max 1000.0
* @max 1800.0
* @decimal 1
* @increment 5
* @group Multicopter Attitude Control
Expand All @@ -373,16 +373,15 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f);
*
* @unit deg/s
* @min 0.0
* @max 1000.0
* @max 1800.0
* @decimal 1
* @increment 5
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f);

/**
* Acro Expo factor
* applied to input of all axis: roll, pitch, yaw
* Acro Expo factor for Roll and Pitch.
*
* 0 Purely linear input curve
* 1 Purely cubic input curve
Expand All @@ -395,8 +394,20 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f);
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we rename MC_ACRO_EXPO_RP for consistency?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Renaming existing params is a pain, and I try to avoid it if at all possible. We can do it within the scope of a larger refactoring.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it should not be that big of a deal in this case. You can change the name if you want.


/**
* Acro SuperExpo factor
* applied to input of all axis: roll, pitch, yaw
* Acro Expo factor for Yaw.
*
* 0 Purely linear input curve
* 1 Purely cubic input curve
*
* @min 0
* @max 1
* @decimal 2
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f);

/**
* Acro SuperExpo factor for Roll and Pitch.
*
* 0 Pure Expo function
* 0.7 resonable shape enhancement for intuitive stick feel
Expand All @@ -409,6 +420,20 @@ PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
*/
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

MC_ACRO_SUPEXPO_RP ?


/**
* Acro SuperExpo factor for 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_SUPEXPOY, 0.7f);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would expect this to be MC_ACRO_SUPEXPO_Y for consistency and readability

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As I wrote:

The limitation of 16 chars did not allow to use a better param name.

So yes your suggestion makes more sense, but exceeds the maximum length unfortunately.


/**
* Threshold for Rattitude mode
*
Expand Down