Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

mc_pos_control: Release estimator speed limit only when speed demand is significant

Browse Source 
This prevents unexpected increases in stick sensitivity when stick is moved after periods of inactivity.
This commit is contained in:
Paul Riseborough
2018-01-24 21:42:46 +11:00
committed by ChristophTobler
parent 6ef43e5b75
commit eed8b00857
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+10 -2
View File
@@ -292,6 +292,7 @@ private:
float _man_yaw_offset; /**< current yaw offset in manual mode */ float _man_yaw_offset; /**< current yaw offset in manual mode */
float _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */ float _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */
bool _vel_sp_significant; /** true when the velocity setpoint is over 50% of the _vel_max_xy limit */
float _acceleration_state_dependent_xy; /**< acceleration limit applied in manual mode */ float _acceleration_state_dependent_xy; /**< acceleration limit applied in manual mode */
float _acceleration_state_dependent_z; /**< acceleration limit applied in manual mode in z */ float _acceleration_state_dependent_z; /**< acceleration limit applied in manual mode in z */
float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */ float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */
@@ -472,6 +473,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_yaw_takeoff(0.0f), _yaw_takeoff(0.0f),
_man_yaw_offset(0.0f), _man_yaw_offset(0.0f),
_vel_max_xy(0.0f), _vel_max_xy(0.0f),
_vel_sp_significant(false),
_acceleration_state_dependent_xy(0.0f), _acceleration_state_dependent_xy(0.0f),
_acceleration_state_dependent_z(0.0f), _acceleration_state_dependent_z(0.0f),
_manual_jerk_limit_xy(1.0f), _manual_jerk_limit_xy(1.0f),
@@ -2536,6 +2538,9 @@ MulticopterPositionControl::calculate_velocity_setpoint()
/* make sure velocity setpoint is constrained in all directions (xyz) */ /* make sure velocity setpoint is constrained in all directions (xyz) */
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
/* check if the velocity demand is significant */
_vel_sp_significant = vel_norm_xy > 0.5f * _vel_max_xy;
if (vel_norm_xy > _vel_max_xy) { if (vel_norm_xy > _vel_max_xy) {
_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy; _vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy; _vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy;
@@ -3079,13 +3084,13 @@ MulticopterPositionControl::task_main()
/* set default max velocity in xy to vel_max /* set default max velocity in xy to vel_max
* Apply estimator limits if applicable */ * Apply estimator limits if applicable */
if (_local_pos.vxy_max > 0.0f) { if (_local_pos.vxy_max > 0.001f) {
// use the minimum of the estimator and user specified limit // use the minimum of the estimator and user specified limit
_vel_max_xy = fminf(_params.vel_max_xy, _local_pos.vxy_max); _vel_max_xy = fminf(_params.vel_max_xy, _local_pos.vxy_max);
// Allow for a minimum of 0.3 m/s for repositioning // Allow for a minimum of 0.3 m/s for repositioning
_vel_max_xy = fmaxf(_vel_max_xy, 0.3f); _vel_max_xy = fmaxf(_vel_max_xy, 0.3f);
} else { } else if (_vel_sp_significant) {
// raise the limit at a constant rate up to the user specified value // raise the limit at a constant rate up to the user specified value
if (_vel_max_xy >= _params.vel_max_xy) { if (_vel_max_xy >= _params.vel_max_xy) {
_vel_max_xy = _params.vel_max_xy; _vel_max_xy = _params.vel_max_xy;
@@ -3122,6 +3127,9 @@ MulticopterPositionControl::task_main()
* TODO: we need a defined setpoint to do this properly especially when adjusting the mixer */ * TODO: we need a defined setpoint to do this properly especially when adjusting the mixer */
_att_sp.thrust = 0.0f; _att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time(); _att_sp.timestamp = hrt_absolute_time();
/* reset velocity limit */
_vel_max_xy = _params.vel_max_xy;
} }
/* reset setpoints and integrators VTOL in FW mode */ /* reset setpoints and integrators VTOL in FW mode */