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,10 +3300,11 @@ 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) {
+				/* 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 */
 
-					/* 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);
 
@@ -3317,12 +3318,6 @@ MulticopterPositionControl::task_main()
 									  _pos_sp_triplet.next.z, NAN, NAN, NAN, NAN, NAN, NAN, _pos_sp_triplet.next.yaw, NAN);
 				}
 
-				} else {
-
-					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);
-				}
-
 				/* publish local position setpoint */
 				if (_local_pos_sp_pub != nullptr) {
 					orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);