diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..60747854cd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -177,6 +177,10 @@ private: param_t man_pitch_max; param_t man_yaw_max; param_t mc_att_yaw_p; + param_t hold_xy_dz; + param_t hold_z_dz; + param_t hold_max_xy; + param_t hold_max_z; } _params_handles; /**< handles for interesting parameters */ struct { @@ -189,6 +193,10 @@ private: float man_pitch_max; float man_yaw_max; float mc_att_yaw_p; + float hold_xy_dz; + float hold_z_dz; + float hold_max_xy; + float hold_max_z; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -206,6 +214,10 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; bool _mode_auto; + bool _pos_hold_engaged; + bool _alt_hold_engaged; + bool _run_pos_control; + bool _run_alt_control; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -213,7 +225,6 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; - math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -325,7 +336,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_pos_sp(true), _reset_alt_sp(true), - _mode_auto(false) + _mode_auto(false), + _pos_hold_engaged(false), + _alt_hold_engaged(false), + _run_pos_control(true), + _run_alt_control(true) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); @@ -354,7 +369,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); - _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -377,6 +391,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); + _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); + _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); + _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); + _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); + /* fetch initial parameter values */ parameters_update(true); @@ -464,6 +483,16 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; + param_get(_params_handles.hold_xy_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_xy_dz = v; + param_get(_params_handles.hold_z_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_z_dz = v; + param_get(_params_handles.hold_max_xy, &v); + _params.hold_max_xy = (v < 0.0f ? 0.0f : v); + param_get(_params_handles.hold_max_z, &v); + _params.hold_max_z = (v < 0.0f ? 0.0f : v); _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -584,9 +613,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -596,7 +625,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -627,31 +656,20 @@ MulticopterPositionControl::limit_pos_sp_offset() void MulticopterPositionControl::control_manual(float dt) { - _sp_move_rate.zero(); + math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range + req_vel_sp.zero(); if (_control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with throttle stick */ - _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + /* set vertical velocity setpoint with throttle stick */ + req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D } if (_control_mode.flag_control_position_enabled) { - /* move position setpoint with roll/pitch stick */ - _sp_move_rate(0) = _manual.x; - _sp_move_rate(1) = _manual.y; + /* set horizontal velocity setpoint with roll/pitch stick */ + req_vel_sp(0) = _manual.x; + req_vel_sp(1) = _manual.y; } - /* limit setpoint move rate */ - float sp_move_norm = _sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - _sp_move_rate /= sp_move_norm; - } - - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); - if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -662,30 +680,70 @@ MulticopterPositionControl::control_manual(float dt) reset_pos_sp(); } - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); + /* limit velocity setpoint */ + float req_vel_sp_norm = req_vel_sp.length(); - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + if (req_vel_sp_norm > 1.0f) { + req_vel_sp /= req_vel_sp_norm; } - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity + + /* + * assisted velocity mode: user controls velocity, but if velocity is small enough, position + * hold is activated for the corresponding axis + */ + + /* horizontal axes */ + if (_control_mode.flag_control_position_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) + { + if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || + (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) + { + _pos_hold_engaged = true; + } + } + else { + _pos_hold_engaged = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + + /* set requested velocity setpoint */ + if (!_pos_hold_engaged) { + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ + _vel_sp(0) = req_vel_sp_scaled(0); + _vel_sp(1) = req_vel_sp_scaled(1); + } } - float pos_sp_offs_norm = pos_sp_offs.length(); + /* vertical axis */ + if (_control_mode.flag_control_altitude_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) + { + if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) + { + _alt_hold_engaged = true; + } + } + else { + _alt_hold_engaged = false; + _pos_sp(2) = _pos(2); + } - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + /* set requested velocity setpoint */ + if (!_alt_hold_engaged) { + _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ + _vel_sp(2) = req_vel_sp_scaled(2); + } } } @@ -710,8 +768,10 @@ MulticopterPositionControl::control_offboard(float dt) reset_pos_sp(); /* set position setpoint move rate */ - _sp_move_rate(0) = _pos_sp_triplet.current.vx; - _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _vel_sp(0) = _pos_sp_triplet.current.vx; + _vel_sp(1) = _pos_sp_triplet.current.vy; + + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } if (_pos_sp_triplet.current.yaw_valid) { @@ -728,15 +788,10 @@ MulticopterPositionControl::control_offboard(float dt) reset_alt_sp(); /* set altitude setpoint move rate */ - _sp_move_rate(2) = _pos_sp_triplet.current.vz; + _vel_sp(2) = _pos_sp_triplet.current.vz; + + _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } - - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); - - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; - } else { reset_pos_sp(); reset_alt_sp(); @@ -1020,7 +1075,11 @@ MulticopterPositionControl::task_main() _vel(2) = _local_pos.vz; _vel_ff.zero(); - _sp_move_rate.zero(); + + /* by default, run position/altitude controller. the control_* functions + * can disable this and run velocity controllers directly in this cycle */ + _run_pos_control = true; + _run_alt_control = true; /* select control source */ if (_control_mode.flag_control_manual_enabled) { @@ -1060,10 +1119,15 @@ MulticopterPositionControl::task_main() } } else { - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; + /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ + if (_run_pos_control) { + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + } - _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + if (_run_alt_control) { + _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); + } /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + @@ -1080,17 +1144,23 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; - } - if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; + } + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } + if (!_control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = 0.0f; + } + /* use constant descend rate when landing, ignore altitude setpoint */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; @@ -1148,7 +1218,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index d29d11c65d..09843de1f1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -265,3 +265,41 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); */ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); +/** + * Deadzone of X,Y sticks where position hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f); + +/** + * Deadzone of Z stick where altitude hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); +