From fd3889b5a6a268de2fd5876bb26765419a7a4eb9 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 14 Mar 2017 10:28:19 +0100 Subject: [PATCH] mc pos control: auto handling such that it does not use slewrate when goint to pos --- .../mc_pos_control/mc_pos_control_main.cpp | 115 ++++++++---------- 1 file changed, 53 insertions(+), 62 deletions(-) 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 eec8f6e7c8..43bb8dae4d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1376,9 +1376,11 @@ void MulticopterPositionControl::control_auto(float dt) bool current_setpoint_valid = false; bool previous_setpoint_valid = false; + bool next_setpoint_valid = false; math::Vector<3> prev_sp; math::Vector<3> curr_sp; + math::Vector<3> next_sp; if (_pos_sp_triplet.current.valid) { @@ -1408,6 +1410,21 @@ void MulticopterPositionControl::control_auto(float dt) } } + + if (_pos_sp_triplet.next.valid) { + map_projection_project(&_ref_pos, + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); + + if (PX4_ISFINITE(next_sp(0)) && + PX4_ISFINITE(next_sp(1)) && + PX4_ISFINITE(next_sp(2))) { + next_setpoint_valid = true; + } + } + + if (current_setpoint_valid && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) { @@ -1421,67 +1438,52 @@ void MulticopterPositionControl::control_auto(float dt) cruising_speed_xy, cruising_speed_z); - math::Vector<3> scale = _params.pos_p.edivide(cruising_speed); - /* convert current setpoint to scaled space */ - math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> scale = _params.pos_p.edivide(cruising_speed); - /* by default use current setpoint as is */ - math::Vector<3> pos_sp_s = curr_sp_s; + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); - if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) && - previous_setpoint_valid) { + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; - /* follow "previous - current" line */ + /* 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(); - if ((curr_sp - prev_sp).length() > MIN_DIST) { + /* we are close to current setpoint */ + if (curr_pos_s_len < 1.0f) { - /* 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(); + if ((next_sp - curr_sp).length() > MIN_DIST) { - if (curr_pos_s_len < 1.0f) { - /* copter is closer to waypoint than unit radius */ - /* check next waypoint and use it to avoid slowing down when passing via waypoint */ - if (_pos_sp_triplet.next.valid) { - math::Vector<3> next_sp; - map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); - next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); + math::Vector<3> next_sp_s = next_sp.emult(scale); - if ((next_sp - curr_sp).length() > MIN_DIST) { - math::Vector<3> next_sp_s = next_sp.emult(scale); + /* 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(); - /* 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(); + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; - /* cos(a) * curr_next, a = angle between current and next trajectory segments */ - float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; - /* cos(b), b = angle pos - curr_sp - prev_sp */ - float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); - if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { - float curr_next_s_len = curr_next_s.length(); - - /* 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; - } - - /* 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; - } + /* 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; } + + /* 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 { @@ -1493,22 +1495,11 @@ void MulticopterPositionControl::control_auto(float dt) } } } + + /* scale back */ + _pos_sp = pos_sp_s.edivide(scale); } - /* 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 result back to normal space */ - _pos_sp = pos_sp_s.edivide(scale); - /* update yaw setpoint if needed */ if (_pos_sp_triplet.current.yawspeed_valid