FixedWingPositionControl: set waypoint straight ahead for front trans…

…ition - this fixes the case where the navigator publishes a loiter waypoint or any waypoint which is too close to the vehicle. Signed-off-by: RomanBapst <[email protected]>
PX4 · Apr 27, 2020 · 2b276a3 · 2b276a3
1 parent 0779a05
commit 2b276a3
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 17 deletions.
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -615,30 +615,52 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 		_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
 
-		/* current waypoint (the one currently heading for) */
-		Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
+		Vector2f curr_wp{0.0f, 0.0f};
+		Vector2f prev_wp{0.0f, 0.0f};
 
-		/* Initialize attitude controller integrator reset flags to 0 */
-		_att_sp.roll_reset_integral = false;
-		_att_sp.pitch_reset_integral = false;
-		_att_sp.yaw_reset_integral = false;
+		if (_vehicle_status.in_transition_to_fw) {
 
-		/* previous waypoint */
-		Vector2f prev_wp{0.0f, 0.0f};
+			if (!PX4_ISFINITE(_transition_waypoint(0))) {
+				double lat_transition, lon_transition;
+				// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
+				// during the transition
+				waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
+								   &lon_transition);
+
+				_transition_waypoint(0) = (float)lat_transition;
+				_transition_waypoint(1) = (float)lon_transition;
+			}
 
-		if (pos_sp_prev.valid) {
-			prev_wp(0) = (float)pos_sp_prev.lat;
-			prev_wp(1) = (float)pos_sp_prev.lon;
+
+			curr_wp = prev_wp = _transition_waypoint;
 
 		} else {
-			/*
-			 * No valid previous waypoint, go for the current wp.
-			 * This is automatically handled by the L1 library.
-			 */
-			prev_wp(0) = (float)pos_sp_curr.lat;
-			prev_wp(1) = (float)pos_sp_curr.lon;
+			/* current waypoint (the one currently heading for) */
+			curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
+
+			if (pos_sp_prev.valid) {
+				prev_wp(0) = (float)pos_sp_prev.lat;
+				prev_wp(1) = (float)pos_sp_prev.lon;
+
+			} else {
+				/*
+				 * No valid previous waypoint, go for the current wp.
+				 * This is automatically handled by the L1 library.
+				 */
+				prev_wp(0) = (float)pos_sp_curr.lat;
+				prev_wp(1) = (float)pos_sp_curr.lon;
+			}
+
+
+			/* reset transition waypoint, will be set upon entering front transition */
+			_transition_waypoint.setNaN();
 		}
 
+		/* Initialize attitude controller integrator reset flags to 0 */
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
+
 		float mission_airspeed = _param_fw_airspd_trim.get();
 
 		if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -253,6 +253,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	bool _vtol_tailsitter{false};
 
+	Vector2f _transition_waypoint{NAN, NAN};
+
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
 	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state