@ -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 */