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

mc_pos_control: moved responsibility of the x,y stick deadzone to a mathematical function

Browse Source 
to prevent a setpoint step when moving the stick over the border of the deadzone
and to enable very small inputs even from a control stick that needs a big deadzone
This commit is contained in:
Matthias Grob
2017-02-25 11:05:53 +01:00
committed by Lorenz Meier
parent ad3d0391ab
commit 8fd27fddcd
2 changed files with 28 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Expo.hpp
+23 -1
View File
@@ -35,7 +35,7 @@
* @file Expo.hpp * @file Expo.hpp
* *
* So called exponential curve function implementation. * So called exponential curve function implementation.
* It's essentially a linear combination between a linear and a cubic function. * It is essentially a linear combination between a linear and a cubic function.
*/ */
#pragma once #pragma once
@@ -46,6 +46,11 @@
namespace math namespace math
{ {
// Type-safe signum function
template<typename T> int sign(T val) {
return (T(0) < val) - (val < T(0));
}
template<typename _Tp> template<typename _Tp>
inline const _Tp expo(const _Tp &value, const _Tp &e) inline const _Tp expo(const _Tp &value, const _Tp &e)
{ {
@@ -53,4 +58,21 @@ inline const _Tp expo(const _Tp &value, const _Tp &e)
return (1-e)*x + e*x*x*x; return (1-e)*x + e*x*x*x;
} }
template<typename _Tp>
inline const _Tp deadzone(const _Tp &value, const _Tp &dz)
{
_Tp x = constrain(value ,(_Tp)-1, (_Tp)1);
// Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone
_Tp out = (x-sign(x)*dz)/(1-dz);
// apply the deadzone (values zero around the middle)
return out * (fabsf(x) > dz);
}
template<typename _Tp>
inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
{
_Tp x = constrain(value ,(_Tp)-1, (_Tp)1);
return expo(deadzone(x, dz),e);
}
} }
src/modules/mc_pos_control/mc_pos_control_main.cpp
+5 -5
View File
@@ -950,8 +950,8 @@ MulticopterPositionControl::control_manual(float dt)
if (_control_mode.flag_control_position_enabled) { if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */ /* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp_xy(0) = math::expo(_manual.x, _params.xy_vel_man_expo); req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz);
req_vel_sp_xy(1) = math::expo(_manual.y, _params.xy_vel_man_expo); req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_dz);
/* reset position setpoint to current position if needed */ /* reset position setpoint to current position if needed */
reset_pos_sp(); reset_pos_sp();
@@ -1017,15 +1017,15 @@ MulticopterPositionControl::control_manual(float dt)
if (_pos_hold_engaged) { if (_pos_hold_engaged) {
/* only switch back to velocity control if user moves stick */ /* only switch back to velocity control if user moves stick */
_pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz) _pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON)
&& (fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz); && (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON);
} else { } else {
/* check if we switch to pos_hold_engaged */ /* check if we switch to pos_hold_engaged */
float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
bool smooth_pos_transition = _control_mode.flag_control_position_enabled && bool smooth_pos_transition = _control_mode.flag_control_position_enabled &&
(fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz && fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz) && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) &&
(_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy); (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy);
/* during transition predict setpoint forward */ /* during transition predict setpoint forward */