FW attitude and position control minor code cleanup (#7802)

 - change a little bit to make the code more readable

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1004,17 +1004,6 @@ FixedwingAttitudeControl::task_main()
 					orb_publish_auto(_attitude_setpoint_id, &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
 				}
 
-				float roll_sp = _att_sp.roll_body;
-				float pitch_sp = _att_sp.pitch_body;
-				float yaw_sp = _att_sp.yaw_body;
-				float throttle_sp = _att_sp.thrust;
-				float yaw_manual = 0.0f;
-
-				/* allow manual yaw in manual modes */
-				if (_vcontrol_mode.flag_control_manual_enabled) {
-					yaw_manual = _manual.r;
-				}
-
 				/* reset integrals where needed */
 				if (_att_sp.roll_reset_integral) {
 					_roll_ctrl.reset_integrator();
@@ -1041,6 +1030,11 @@ FixedwingAttitudeControl::task_main()
 					_wheel_ctrl.reset_integrator();
 				}
 
+				float roll_sp = _att_sp.roll_body;
+				float pitch_sp = _att_sp.pitch_body;
+				float yaw_sp = _att_sp.yaw_body;
+				float throttle_sp = _att_sp.thrust;
+
 				/* Prepare data for attitude controllers */
 				struct ECL_ControlData control_input = {};
 				control_input.roll = _roll;
@@ -1125,8 +1119,10 @@ FixedwingAttitudeControl::task_main()
 						_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw :
 								_parameters.trim_yaw;
 
-						/* add in manual rudder control */
-						_actuators.control[actuator_controls_s::INDEX_YAW] += yaw_manual;
+						/* add in manual rudder control in manual modes */
+						if (_vcontrol_mode.flag_control_manual_enabled) {
+							_actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r;
+						}
 
 						if (!PX4_ISFINITE(yaw_u)) {
 							_yaw_ctrl.reset_integrator();

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -829,16 +829,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				wp_distance_save = 0.0f;
 			}
 
-			// create virtual waypoint which is on the desired flight path but
-			// some distance behind landing waypoint. This will make sure that the plane
-			// will always follow the desired flight path even if we get close or past
-			// the landing waypoint
-			double lat{0.0f};
-			double lon{0.0f};
-			create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon,
-							   pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon);
-
-			math::Vector<2> curr_wp_shifted {(float)lat, (float)lon};
 
 			// we want the plane to keep tracking the desired flight path until we start flaring
 			// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds