mc_pos_control: refactor the update of the desired waypoints for avoidance

to eliminate duplicated code

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 */