From fc0bdfb6f5021f5cc2d155ea3cee9f5d5102cdef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:42:12 +0200 Subject: mc_pos_control: trajectory following, using previous and next waypoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 184 +++++++++++++++++---- 1 file changed, 156 insertions(+), 28 deletions(-) (limited to 'src') 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 ecc40428d..488ca924a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -76,6 +76,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f +#define MIN_DIST 0.01f /** * Multicopter position control app start / stop handling function @@ -229,6 +230,14 @@ private: */ void control_offboard(float dt); + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(); + /** * Select between barometric and global (AMSL) altitudes */ @@ -647,6 +656,152 @@ MulticopterPositionControl::control_offboard(float dt) } } +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto() +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + + /* by default use current setpoint as is */ + _pos_sp = curr_sp; + + if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + math::Vector<3> x; + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 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); + + 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 = (curr_sp_s - prev_sp_s).normalized(); + + /* cos(a) = angle between current and next trajectory segments */ + float cos_a = prev_curr_s_norm * curr_next_s; + + /* cos(b) = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a * 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 += pos_ff.edivide(scale); + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { + x = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { + x = curr_sp_s; + } + + x = pos_s + (x - pos_s).normalized(); + } + + /* scale result back to normal space */ + _pos_sp = x.edivide(scale); + } + } + } + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -757,34 +912,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; - - /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); - _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); - - /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet.current.yaw)) { - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - } - - } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); - } + control_auto(); } /* fill local position setpoint */ -- cgit v1.2.3 From f31c3243b05fae6ffa4ab8cccb8e009470ca1294 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Aug 2014 23:28:48 +0200 Subject: mc_pos_control: navigation fixes, smooth position setpoint movements --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 77 ++++++++++++---------- 1 file changed, 44 insertions(+), 33 deletions(-) (limited to 'src') 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 488ca924a..847a92151 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -236,7 +236,7 @@ private: /** * Set position setpoint for AUTO */ - void control_auto(); + void control_auto(float dt); /** * Select between barometric and global (AMSL) altitudes @@ -682,7 +682,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } void -MulticopterPositionControl::control_auto() +MulticopterPositionControl::control_auto(float dt) { bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -703,8 +703,14 @@ MulticopterPositionControl::control_auto() &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + /* by default use current setpoint as is */ - _pos_sp = curr_sp; + math::Vector<3> pos_sp_s = curr_sp_s; if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ @@ -715,16 +721,13 @@ MulticopterPositionControl::control_auto() prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { - /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ - math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); /* find X - cross point of L1 sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); math::Vector<3> prev_sp_s = prev_sp.emult(scale); - math::Vector<3> curr_sp_s = curr_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(); - math::Vector<3> x; if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than L1 radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ @@ -740,56 +743,64 @@ MulticopterPositionControl::control_auto() /* calculate angle prev - curr - next */ math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; - math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); - /* cos(a) = angle between current and next trajectory segments */ - float cos_a = 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) = angle pos - curr_sp - prev_sp */ + /* 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 > 0.0f && cos_b > 0.0f) { + 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 L1 radius, limit it */ if (curr_next_s_len > 1.0f) { - cos_a /= curr_next_s_len; + cos_a_curr_next /= curr_next_s_len; } /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + 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 += pos_ff.edivide(scale); + pos_sp_s += pos_ff; } } } } else { - bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); if (near) { /* L1 sphere crosses trajectory */ } else { /* copter is too far from trajectory */ /* if copter is behind prev waypoint, go directly to prev waypoint */ - if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { - x = prev_sp_s; + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; } /* if copter is in front of curr waypoint, go directly to curr waypoint */ - if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { - x = curr_sp_s; + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; } - x = pos_s + (x - pos_s).normalized(); + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); } - - /* scale result back to normal space */ - _pos_sp = x.edivide(scale); } } } + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; + float d_pos_s_len = d_pos_s.length(); + if (d_pos_s_len > dt) { + pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; @@ -912,7 +923,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - control_auto(); + control_auto(dt); } /* fill local position setpoint */ @@ -975,14 +986,14 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled) { - /* limit 3D speed only in non-manual modes */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; - } - } +// if (!_control_mode.flag_control_manual_enabled) { +// /* limit 3D speed only in non-manual modes */ +// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); +// +// if (vel_sp_norm > 1.0f) { +// _vel_sp /= vel_sp_norm; +// } +// } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); -- cgit v1.2.3 From 8ad1aa789b6f10769dd9977151c9a39971b1ebaf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:13:29 +0200 Subject: mc_pos_control: reset position setpoint on entering to AUTO mode --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) (limited to 'src') 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 847a92151..01fdb4c65 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,7 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; + bool _mode_auto; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -292,7 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_timestamp(0), _reset_pos_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _mode_auto(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -684,6 +686,13 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f void MulticopterPositionControl::control_auto(float dt) { + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -807,9 +816,7 @@ MulticopterPositionControl::control_auto(float dt) } } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); + /* no waypoint, do nothing, setpoint was already reset */ } } @@ -916,10 +923,12 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); + _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); + _mode_auto = false; } else { /* AUTO */ @@ -1271,9 +1280,9 @@ MulticopterPositionControl::task_main() /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; + _mode_auto = false; reset_int_z = true; reset_int_xy = true; - } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From 132c9180ead0c64c2edcc36dec2bf8896679a040 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:47:56 +0200 Subject: mc_pos_control: move position offset limiting to separate method --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 ++++++++++++++++++++++ 1 file changed, 28 insertions(+) (limited to 'src') 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 01fdb4c65..ce26f7dc2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -221,6 +221,11 @@ private: */ void reset_alt_sp(); + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + /** * Set position setpoint using manual control */ @@ -544,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + 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 (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + 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); + } +} + void MulticopterPositionControl::control_manual(float dt) { -- cgit v1.2.3 From 2d81c2cf466596b7c4f787a52836fd200af2a097 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:43:42 +0400 Subject: mc_pos_control: commented code block removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 1 file changed, 10 deletions(-) (limited to 'src') 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 ce26f7dc2..99deb0d29 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1022,16 +1022,6 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } - -// if (!_control_mode.flag_control_manual_enabled) { -// /* limit 3D speed only in non-manual modes */ -// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); -// -// if (vel_sp_norm > 1.0f) { -// _vel_sp /= vel_sp_norm; -// } -// } - _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); -- cgit v1.2.3 From 84908f8f3db5179ffff0d96d15756ab112535482 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Oct 2014 15:45:02 +0400 Subject: mc_pos_control: AUTO speed limiting bug fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'src') 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 99deb0d29..ec7b2a78f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt) /* move setpoint not faster than max allowed speed */ math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); - math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; - float d_pos_s_len = d_pos_s.length(); - if (d_pos_s_len > dt) { - pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + + /* 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 */ -- cgit v1.2.3