From d39b969e72b523d7fb767abe6c7d6dd67d380026 Mon Sep 17 00:00:00 2001 From: Martina Date: Fri, 6 Apr 2018 09:48:40 +0200 Subject: [PATCH] mc_pos_control: refactor the update of the desired waypoints for avoidance to eliminate duplicated code --- .../mc_pos_control/mc_pos_control_main.cpp | 31 ++++++++----------- 1 file changed, 13 insertions(+), 18 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 a4f0bdea69..ffcca5c65c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -3300,27 +3300,22 @@ MulticopterPositionControl::task_main() _local_pos_sp.vy = _vel_sp(1); _local_pos_sp.vz = _vel_sp(2); - /* desired waypoints for obstacle avoidance */ - if (_pos_sp_triplet.current.valid) { - - /* point_0 containes the current position with the desired velocity */ - update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0), - _vel_sp_desired(1), _vel_sp_desired(2), NAN, NAN, NAN, _yaw, NAN); - - if (_pos_sp_triplet.current.valid) { - update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_1, _pos_sp_triplet.current.x, _pos_sp_triplet.current.y, - _pos_sp_triplet.current.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.current.yaw, NAN); - } + /* desired waypoints for obstacle avoidance: + * point_0 containes the current position with the desired velocity + * point_1 containes _pos_sp_triplet.current if valid + * point_2 containes _pos_sp_triplet.next if valid */ - if (_pos_sp_triplet.next.valid) { - update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_2, _pos_sp_triplet.next.x, _pos_sp_triplet.next.y, - _pos_sp_triplet.next.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN); - } + update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0), + _vel_sp_desired(1), _vel_sp_desired(2), NAN, NAN, NAN, _yaw, NAN); - } else { + if (_pos_sp_triplet.current.valid) { + update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_1, _pos_sp_triplet.current.x, _pos_sp_triplet.current.y, + _pos_sp_triplet.current.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.current.yaw, NAN); + } - update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_0, _pos(0), _pos(1), _pos(2), _vel_sp_desired(0), - _vel_sp_desired(1), _vel_sp_desired(2), NAN, NAN, NAN, _yaw, NAN); + if (_pos_sp_triplet.next.valid) { + update_avoidance_waypoint_desired(trajectory_waypoint_s::POINT_2, _pos_sp_triplet.next.x, _pos_sp_triplet.next.y, + _pos_sp_triplet.next.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN); } /* publish local position setpoint */