fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -292,6 +292,7 @@ private:
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 */
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_z; /**< acceleration limit applied in manual mode in z */
float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */
@ -472,6 +473,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_yaw_takeoff(0.0f),
_man_yaw_offset(0.0f),
_vel_max_xy(0.0f),
_vel_sp_significant(false),
_acceleration_state_dependent_xy(0.0f),
_acceleration_state_dependent_z(0.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) */
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) {
_vel_sp(0) = _vel_sp(0) * _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
* 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
_vel_max_xy = fminf(_params.vel_max_xy, _local_pos.vxy_max);
// Allow for a minimum of 0.3 m/s for repositioning
_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
if (_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 */
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* reset velocity limit */
_vel_max_xy = _params.vel_max_xy;
}
/* reset setpoints and integrators VTOL in FW mode */