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 0be11652c9..870f6bec1b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -144,6 +144,7 @@ private: control::BlockParamFloat _acceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */ control::BlockParamFloat _deceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */ control::BlockParamFloat _target_threshold_xy; /**< distance threshold for slowdown close to target during mission */ + control::BlockParamFloat _min_cruise_speed; /**< minimum cruising speed when passing waypoint */ control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed*/ control::BlockParamFloat _takeoff_ramp_time; /**< time contant for smooth takeoff ramp */ @@ -407,6 +408,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _target_threshold_xy(this, "TARGET_THRE"), _velocity_hor_manual(this, "VEL_MAN_MAX", true), _takeoff_ramp_time(this, "TKO_RAMP_T", true), + _min_cruise_speed(this, "CRUISE_MIN", true), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), @@ -862,25 +864,6 @@ MulticopterPositionControl::limit_altitude() } } -void -MulticopterPositionControl::limit_vel_xy_gradually() -{ - /* - * the max velocity is defined by the linear line - * with x= (curr_sp - pos) and y = _vel_sp with min limit of 0.01 - */ - math::Vector<3> dist = _curr_pos_sp - _pos; - float slope = (get_cruising_speed_xy() - 0.01f) / _target_threshold_xy.get(); - float vel_limit = slope * sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) + 0.01f; - float vel_mag_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); - - if (vel_mag_xy <= vel_limit) { - return; - } - - _vel_sp(0) = _vel_sp(0) / vel_mag_xy * vel_limit; - _vel_sp(1) = _vel_sp(1) / vel_mag_xy * vel_limit; -} bool MulticopterPositionControl::in_auto_takeoff() @@ -1285,10 +1268,17 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c /* project center of sphere on line */ /* normalized AB */ math::Vector<3> ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + ab_norm.normalize(); math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); float cd_len = (sphere_c - d).length(); + //PX4_INFO("sphere_r: %.5f",(double)sphere_r); + //PX4_INFO("cd len: %.5f",(double)cd_len); if (sphere_r > cd_len) { /* we have triangle CDX with known CD and CX = R, find DX */ float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); @@ -1305,16 +1295,21 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c return true; } else { + + PX4_INFO("return false"); /* have no roots, return D */ res = d; /* go directly to line */ /* previous waypoint is still in front of us */ if ((sphere_c - line_a) * ab_norm < 0.0f) { + PX4_INFO("previous is in front"); res = line_a; } /* target waypoint is already behind us */ if ((sphere_c - line_b) * ab_norm > 0.0f) { + PX4_INFO("target is behind"); + res = line_b; } @@ -1405,11 +1400,6 @@ void MulticopterPositionControl::control_auto(float dt) } } - /* set velocity limit if close to current setpoint and no next setpoint available */ - math::Vector<3> dist = _curr_pos_sp - _pos; - _limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) - && (sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) <= _target_threshold_xy.get()); - if (current_setpoint_valid && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) { @@ -1419,92 +1409,100 @@ void MulticopterPositionControl::control_auto(float dt) /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> cruising_speed(cruising_speed_xy, cruising_speed_xy, cruising_speed_z); + /* if previous is valid, we want to follow line */ if (previous_setpoint_valid && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET)) { - math::Vector<3> scale = _params.pos_p.edivide(cruising_speed); - - /* convert current setpoint to scaled space */ - math::Vector<3> curr_sp_s = _curr_pos_sp.emult(scale); /* by default use current setpoint as is */ - math::Vector<3> pos_sp_s = curr_sp_s; + math::Vector<3> pos_sp = _curr_pos_sp; const float minimum_dist = 0.01f; if ((_curr_pos_sp - prev_sp).length() > minimum_dist) { - /* find X - cross point of unit sphere and trajectory */ - math::Vector<3> pos_s = _pos.emult(scale); - math::Vector<3> prev_sp_s = prev_sp.emult(scale); - math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; - math::Vector<3> curr_pos_s = pos_s - curr_sp_s; - float curr_pos_s_len = curr_pos_s.length(); + /* unit vector from previous to current */ + matrix::Vector2f unit_prev_to_current((_curr_pos_sp(0) - prev_sp(0)), (_curr_pos_sp(1) - prev_sp(1))); + unit_prev_to_current.normalize(); - /* we are close to current setpoint */ - if (curr_pos_s_len < 1.0f) { + /* orthogonal distance from current position to unit_prev_to_current */ + matrix::Vector2f closest_point = matrix::Vector2f(prev_sp(0), prev_sp(1)) + unit_prev_to_current * + (matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1))) * unit_prev_to_current); - /* if next is valid, we want to have smooth transition */ - if (next_setpoint_valid && (next_sp - _curr_pos_sp).length() > minimum_dist) { + /* check if we need to adjust position setpoint based on cruise velocity */ + matrix::Vector2f vec_pos_to_current = matrix::Vector2f((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1))); + matrix::Vector2f vec_prev_to_pos = matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1))); - math::Vector<3> next_sp_s = next_sp.emult(scale); + /* slow down */ + if (vec_pos_to_current.length() < _target_threshold_xy.get()) { - /* calculate angle prev - curr - next */ - math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; - math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + /* angle prev-current-next defines speed close to waypoint */ + if (next_setpoint_valid) { - /* cos(a) * curr_next, a = angle between current and next trajectory segments */ - float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + /* unit vector to from current to next */ + matrix::Vector2f unit_current_to_next((next_sp(0) - pos_sp(0)), (next_sp(1) - pos_sp(1))); + unit_current_to_next = (unit_current_to_next.length() > 0.0f) ? unit_current_to_next.normalized() : + unit_current_to_next; - /* cos(b), b = angle pos - _curr_pos_sp - prev_sp */ - float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + /* angle goes from 0 to 2 with 0 being large angle, 2 being tight angle: 0 = PI ; 2 = PI*0 */ + float angle = unit_current_to_next * (-unit_prev_to_current) + 1.0f; - if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { - float curr_next_s_len = curr_next_s.length(); + /* velocity close to target adjusted to angle + * TODO: add explanation*/ + float M = get_cruising_speed_xy() / 4.0f; + float a = -((M - get_cruising_speed_xy()) * (M - get_cruising_speed_xy())) / (2.0f * M - get_cruising_speed_xy() - + _min_cruise_speed.get()); + float c = get_cruising_speed_xy() - a; + float b = (M - c) / a; + float vel_close = a * powf(b, angle) + c; - /* if curr - next distance is larger than unit radius, limit it */ - if (curr_next_s_len > 1.0f) { - cos_a_curr_next /= curr_next_s_len; - } + float slope = (get_cruising_speed_xy() - vel_close) / _target_threshold_xy.get(); + cruising_speed_xy = slope * vec_pos_to_current.length() + vel_close; - /* feed forward position setpoint offset */ - math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); - pos_sp_s += pos_ff; - } + + } else { + /* we want to stop at next waypoint */ + float slope = (get_cruising_speed_xy() - _min_cruise_speed.get()) / _target_threshold_xy.get(); + cruising_speed_xy = slope * vec_pos_to_current.length() + _min_cruise_speed.get(); } - } else { - /* if not close to current setpoint, check if we are within cross_sphere_line */ - bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); - if (!near) { - /* we're far away from trajectory, pos_sp_s is set to the nearest point on the trajectory */ - pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + /* accelerate */ + + } else if (vec_prev_to_pos.length() < _target_threshold_xy.get()) { + float slope = (get_cruising_speed_xy() - _min_cruise_speed.get()) / _target_threshold_xy.get(); + cruising_speed_xy = slope * vec_prev_to_pos.length() + _min_cruise_speed.get(); + } + + + /* we want position setpoint not farther away then cruise speed */ + if (vec_pos_to_current.length() * _params.pos_p(0) > cruising_speed_xy) { + + /* we adjust position setpoint */ + matrix::Vector2f vec_pos_to_closest = closest_point - matrix::Vector2f(_pos(0), _pos(1)); + float vel_orthogonal = vec_pos_to_closest.length() * _params.pos_p(0); + + if (vel_orthogonal <= cruising_speed_xy) { + float vel_along_track = sqrtf(cruising_speed_xy * cruising_speed_xy - vel_orthogonal * vel_orthogonal); + pos_sp(0) = closest_point(0) + unit_prev_to_current(0) * vel_along_track / _params.pos_p(0); + pos_sp(1) = closest_point(1) + unit_prev_to_current(1) * vel_along_track / _params.pos_p(0); + + } else { + pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(0); + pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(1); + } } + } - /* move setpoint not faster than max allowed speed */ - math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); - - /* difference between current and desired position setpoints, 1 = max speed */ - math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); - float d_pos_m_len = d_pos_m.length(); - - if (d_pos_m_len > dt) { - pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); - } - - /* scale back */ - _pos_sp = pos_sp_s.edivide(scale); + _pos_sp = pos_sp; } else { - /* we just have a current setpoint that we want to go to */ + /* we just have a current setpoint that we want to go to */; _pos_sp = _curr_pos_sp; /* set max velocity to cruise */ @@ -1620,11 +1618,6 @@ MulticopterPositionControl::do_control(float dt) _run_pos_control = true; _run_alt_control = true; - /* if not in auto mode, we reset limit_vel_xy flag */ - if (_control_mode.flag_control_manual_enabled || _control_mode.flag_control_offboard_enabled) { - _limit_vel_xy = false; - } - if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); @@ -1689,11 +1682,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt) } } - /* we are close to target and want to limit velocity in xy */ - if (_limit_vel_xy) { - limit_vel_xy_gradually(); - } - if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; } 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 bf4859af7a..da1f938caf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -229,7 +229,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** - * Nominal horizontal velocity in mission + * Maximum horizontal velocity in mission * * Normal horizontal velocity in AUTO modes (includes * also RTL / hold / etc.) and endpoint for @@ -245,7 +245,23 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); /** - * Nominal horizontal velocity for manual controlled mode + * Minimum horizontal velocity in mission + * + * Normal horizontal velocity in AUTO modes (includes + * also RTL / hold / etc.) and endpoint for + * position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.5 + * @max 8.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 1.0f); + +/** + * Maximum horizontal velocity setpoint for manual controlled mode * * @unit m/s * @min 3.0