Avoid waypoint following during backtransition which can lead to strong banking (#22642)

* avoid waypoint following during backtransition
- if vehicle overshoots transition, make controller track the prev/current
waypoint line, this avoids large banking maneuvers that delay the transition further

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* navigator: use SETPOINT_TYPE_LAND as setpoint type for VTOL_LAND waypoints
such that fixed wing position controller can distinguish from other types

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FixedWingPositionControl: split vtol backtransition logic into separate method

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* review changes

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* prevent loiter when approaching vtol backtransition and invalidate previous waypoint during mission landing

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* removed unused parameter

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* small renaming of transition mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -686,8 +686,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 		// Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
 		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
+		const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;
 
-		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
+		if (doing_backtransition) {
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITON;
+
+		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
 			if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
 				_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -708,22 +712,14 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
-			if (!_vehicle_status.in_transition_mode) {
+			// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
-
+			// Straight landings are currently only possible in Missions, and there the previous WP
-				// Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
+			// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
-				// Straight landings are currently only possible in Missions, and there the previous WP
+			if (_position_setpoint_previous_valid) {
-				// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
-				if (_position_setpoint_previous_valid) {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
-
-				} else {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
-				}
 
 			} else {
-				_control_mode_current = FW_POSCTRL_MODE_AUTO;
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
-				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
-				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 			}
 
 		} else {
@@ -838,7 +834,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 	position_setpoint_s current_sp = pos_sp_curr;
 	move_position_setpoint_for_vtol_transition(current_sp);
 
-	const uint8_t position_sp_type = handle_setpoint_type(current_sp);
+	const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next);
 
 	_position_sp_type = position_sp_type;
 
@@ -971,7 +967,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 }
 
 uint8_t
-FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
+		const position_setpoint_s &pos_sp_next)
 {
 	uint8_t position_sp_type = pos_sp_curr.type;
 
@@ -986,8 +983,13 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 
 	const float acc_rad = _npfg.switchDistance(500.0f);
 
-	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
+	const bool approaching_vtol_backtransition = _vehicle_status.is_vtol
-	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+			&& pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid
+			&& pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid;
+
+
+	// check if we should switch to loiter but only if we are not expecting a backtransition to happen
+	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) {
 
 		float dist_xy = -1.f;
 		float dist_z = -1.f;
@@ -997,17 +999,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 					   _current_latitude, _current_longitude, _current_altitude,
 					   &dist_xy, &dist_z);
 
-		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+		// Achieve position setpoint altitude via loiter when laterally close to WP.
-			// Achieve position setpoint altitude via loiter when laterally close to WP.
+		// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
-			// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
+		// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
-			// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
+		if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
-			if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
+		    && (dist_z > _param_nav_fw_alt_rad.get())
-			    && (dist_z > _param_nav_fw_alt_rad.get())
+		    && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
-			    && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
-				// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
+			// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
-				position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+			position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-			}
 		}
 	}
 
@@ -2178,6 +2178,53 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	_att_sp.pitch_body = get_tecs_pitch();
 }
 
+void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
+		const position_setpoint_s &pos_sp_prev,
+		const position_setpoint_s &pos_sp_curr)
+{
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
+				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
+
+
+	if (_position_setpoint_previous_valid) {
+		Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
+		navigateLine(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+
+	} else {
+
+		// if we don't have a previous waypoint for line following, then create one using the current position at the
+		// start of the transition
+		if (!_lpos_where_backtrans_started.isAllFinite()) {
+			_lpos_where_backtrans_started = curr_pos_local;
+		}
+
+		navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	}
+
+	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	tecs_update_pitch_throttle(control_interval,
+				   pos_sp_curr.alt,
+				   target_airspeed,
+				   radians(_param_fw_p_lim_min.get()),
+				   radians(_param_fw_p_lim_max.get()),
+				   _param_fw_thr_min.get(),
+				   _param_fw_thr_max.get(),
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get());
+
+	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
+	_att_sp.pitch_body = get_tecs_pitch();
+}
 float
 FixedwingPositionControl::get_tecs_pitch()
 {
@@ -2381,7 +2428,11 @@ FixedwingPositionControl::Run()
 			}
 		}
 
-		_vehicle_status_sub.update(&_vehicle_status);
+		if (_vehicle_status_sub.update(&_vehicle_status)) {
+			if (!_vehicle_status.in_transition_mode) {
+				_lpos_where_backtrans_started = Vector2f(NAN, NAN);
+			}
+		}
 
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
@@ -2474,8 +2525,14 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_TRANSITON: {
+				control_backtransition(control_interval, ground_speed, _pos_sp_triplet.previous,
+						       _pos_sp_triplet.current);
+				break;
+			}
 		}
 
+
 		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
 
 			if (_control_mode.flag_control_manual_enabled) {
@@ -3062,6 +3119,7 @@ float FixedwingPositionControl::getLoadFactor()
 
 }
 
+
 extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[])
 {
 	return FixedwingPositionControl::main(argc, argv);

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -101,6 +101,7 @@
 #ifdef CONFIG_FIGURE_OF_EIGHT
 #include "figure_eight/FigureEight.hpp"
 #include <uORB/topics/figure_eight_status.h>
+
 #endif // CONFIG_FIGURE_OF_EIGHT
 
 using namespace launchdetection;
@@ -222,6 +223,8 @@ private:
 	vehicle_local_position_s _local_pos{};
 	vehicle_status_s _vehicle_status{};
 
+	Vector2f _lpos_where_backtrans_started;
+
 	bool _position_setpoint_previous_valid{false};
 	bool _position_setpoint_current_valid{false};
 	bool _position_setpoint_next_valid{false};
@@ -243,6 +246,7 @@ private:
 		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
+		FW_POSCTRL_MODE_TRANSITON,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
@@ -540,7 +544,8 @@ private:
 	 * @param pos_sp_curr Current position setpoint
 	 * @return Adjusted position setpoint type
 	 */
-	uint8_t	handle_setpoint_type(const position_setpoint_s &pos_sp_curr);
+	uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
+				     const position_setpoint_s &pos_sp_next);
 
 	/* automatic control methods */
 
@@ -685,6 +690,18 @@ private:
 	 */
 	void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
 
+	/**
+	 * @brief Controls flying towards a transition waypoint and then transitioning to MC mode.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_backtransition(const float control_interval, const Vector2f &ground_speed,
+				    const position_setpoint_s &pos_sp_prev,
+				    const position_setpoint_s &pos_sp_curr);
+
 	float get_tecs_pitch();
 	float get_tecs_thrust();
 

src/modules/navigator/mission_base.cpp

@@ -574,10 +574,6 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s
 			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
 
 			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
-
-			// make previous setpoint invalid, such that there will be no prev-current line following
-			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 		}
 
 	} else if (needs_to_land) {

src/modules/navigator/mission_block.cpp

@@ -110,7 +110,8 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 		if (int(_mission_item.params[0]) == 3) {
 			// transition to RW requested, only accept waypoint if vehicle state has changed accordingly
-			return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+			return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+			       && !_navigator->get_vstatus()->in_transition_mode;
 
 		} else if (int(_mission_item.params[0]) == 4) {
 			// transition to FW requested, only accept waypoint if vehicle state has changed accordingly

src/modules/navigator/rtl_direct.cpp

@@ -310,6 +310,7 @@ void RtlDirect::set_rtl_item()
 			dest.alt = loiter_altitude;
 
 			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
+			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 
 			_rtl_state = RTLState::LAND;