Fixed-wing Position Control: Split up control methods for VTOL backtr…

…ansition (#23893) * split methods to control backtransition depening on availablity of position Signed-off-by: RomanBapst <[email protected]> * fixed sign error and replace hardcoded number with constant Signed-off-by: RomanBapst <[email protected]> * make changes such that controller holds initial heading during transition Signed-off-by: RomanBapst <[email protected]> * use reference instead of copy Signed-off-by: RomanBapst <[email protected]> * added comment Signed-off-by: RomanBapst <[email protected]> * flash reduction Signed-off-by: RomanBapst <[email protected]> --------- Signed-off-by: RomanBapst <[email protected]>
PX4 · Dec 16, 2024 · 93c25ef · 93c25ef
1 parent 051ced0
commit 93c25ef
Show file tree

Hide file tree

Showing 3 changed files with 75 additions and 54 deletions.
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -714,7 +714,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
 
 		if (doing_backtransition) {
-			_control_mode_current = FW_POSCTRL_MODE_TRANSITON;
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW;
 
 		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
@@ -766,7 +766,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 		if (doing_backtransition) {
 			// we handle loss of position control during backtransition as a special case
-			_control_mode_current = FW_POSCTRL_MODE_TRANSITON;
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
 
 		} else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
 			   && !_vehicle_status.in_transition_mode) {
@@ -2356,18 +2356,49 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
-void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
-		const position_setpoint_s &pos_sp_curr)
+void FixedwingPositionControl::control_backtransition_heading_hold()
 {
-	const bool is_low_height = checkLowHeightConditions();
+	if (!PX4_ISFINITE(_backtrans_heading)) {
+		_backtrans_heading = _local_pos.heading;
+	}
 
-	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
+	float true_airspeed = _airspeed_eas * _eas2tas;
+
+	if (!_airspeed_valid) {
+		true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas;
+	}
+
+	// we can achieve heading control by setting airspeed and groundspeed vector equal
+	const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed;
+	const Vector2f &ground_speed = airspeed_vector;
+
+	_npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas);
+	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
+
+	Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT;
 
+	navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f));
+
+	const float roll_body = getCorrectedNpfgRollSetpoint();
+
+	const float yaw_body = _backtrans_heading;
+
+	// these values are overriden by transition logic
+	_att_sp.thrust_body[0] = _param_fw_thr_min.get();
+	const float pitch_body = 0.0f;
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
+
+}
+
+void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed,
+		const position_setpoint_s &pos_sp_curr)
+{
 	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.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas);
 	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 
 	// Set the position where the backtransition started the first ime we pass through here.
@@ -2376,30 +2407,14 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
 		_lpos_where_backtrans_started = curr_pos_local;
 	}
 
-	float roll_body{0.0f};
-
-	if (_control_mode.flag_control_position_enabled) {
-		navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-		roll_body = getCorrectedNpfgRollSetpoint();
-	}
-
-	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+	navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	const float roll_body = getCorrectedNpfgRollSetpoint();
 
-	float 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(),
-				   is_low_height);
+	const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
-	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
-	const float pitch_body = get_tecs_pitch();
+	// these values are overriden by transition logic
+	_att_sp.thrust_body[0] = _param_fw_thr_min.get();
+	const float pitch_body = 0.0f;
 
 	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
 	attitude_setpoint.copyTo(_att_sp.q_d);
@@ -2607,6 +2622,7 @@ FixedwingPositionControl::Run()
 			if (!_vehicle_status.in_transition_mode) {
 				// reset position of backtransition start if not in transition
 				_lpos_where_backtrans_started = Vector2f(NAN, NAN);
+				_backtrans_heading = NAN;
 			}
 		}
 
@@ -2703,8 +2719,13 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_TRANSITON: {
-				control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current);
+		case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW: {
+				control_backtransition_line_follow(ground_speed, _pos_sp_triplet.current);
+				break;
+			}
+
+		case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD: {
+				control_backtransition_heading_hold();
 				break;
 			}
 		}

diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -260,7 +260,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
-		FW_POSCTRL_MODE_TRANSITON,
+		FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW,
+		FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
@@ -409,6 +410,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	// VTOL / TRANSITION
 	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
+	float _backtrans_heading{NAN};	// used to lock the initial heading for backtransition with no position control
 
 	// ESTIMATOR RESET COUNTERS
 	uint8_t _xy_reset_counter{0};
@@ -707,15 +709,20 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	 */
 	void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
 
+	/**
+	 * @brief Holds the initial heading during the course of a transition to hover. Used when there is no local
+	 * position to do line following.
+	 */
+	void control_backtransition_heading_hold();
+
 	/**
 	 * @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_curr current position setpoint
 	 */
-	void control_backtransition(const float control_interval, const Vector2f &ground_speed,
-				    const position_setpoint_s &pos_sp_curr);
+	void control_backtransition_line_follow(const Vector2f &ground_speed,
+						const position_setpoint_s &pos_sp_curr);
 
 	float get_tecs_pitch();
 	float get_tecs_thrust();

diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c
@@ -34,8 +34,8 @@
 /**
  * Path navigation roll slew rate limit.
  *
- * The maximum change in roll angle setpoint per second.
- * This limit is applied in all Auto modes, plus manual Position and Altitude modes.
+ * Maximum change in roll angle setpoint per second.
+ * Applied in all Auto modes, plus manual Position & Altitude modes.
  *
  * @unit deg/s
  * @min 0
@@ -48,7 +48,7 @@ PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f);
 /**
  * NPFG period
  *
- * Period of the NPFG control law.
+ * Period of NPFG control law.
  *
  * @unit s
  * @min 1.0
@@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f);
 /**
  * NPFG damping ratio
  *
- * Damping ratio of the NPFG control law.
+ * Damping ratio of NPFG control law.
  *
  * @min 0.10
  * @max 1.00
@@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f);
  * Enable automatic lower bound on the NPFG period
  *
  * Avoids limit cycling from a too aggressively tuned period/damping combination.
- * If set to false, also disables the upper bound NPFG_PERIOD_UB.
+ * If false, also disables upper bound NPFG_PERIOD_UB.
  *
  * @boolean
  * @group FW NPFG Control
@@ -339,8 +339,6 @@ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f);
  *
  * This is critical for detecting when to flare, and should be enabled if possible.
  *
- * NOTE: terrain estimate is currently solely derived from a distance sensor.
- *
  * If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing
  * will be aborted, depending on the criteria set in FW_LND_ABORT.
  *
@@ -428,12 +426,12 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
 /**
  * Low-height threshold for tighter altitude tracking
  *
- * Defines the height (distance to bottom) threshold below which tighter altitude
+ * Height above ground threshold below which tighter altitude
  * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly
  * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC
  * to FW_LND_THRTC_SC*FW_T_ALT_TC.
  *
- * If equal to -1, low-height traking is disabled.
+ * -1 to disable.
  *
  * @unit m
  * @min -1
@@ -451,8 +449,6 @@ PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f);
 /**
  * Maximum descent rate
  *
- * This sets the maximum descent rate that the controller will use.
- *
  * @unit m/s
  * @min 1.0
  * @max 15.0
@@ -579,15 +575,12 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
 PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
 
 /**
- * Speed <--> Altitude priority
+ * Speed <--> Altitude weight
  *
  * Adjusts the amount of weighting that the pitch control
- * applies to speed vs height errors. Setting it to 0.0 will cause the
- * pitch control to control height and ignore speed errors.
- * Setting it to 2.0 will cause the pitch control loop to control speed
- * and ignore height errors. The default value of 1.0 allows the pitch
- * control to simultaneously control height and speed.
- * Set to 2 for gliders.
+ * applies to speed vs height errors.
+ * 0 -> control height only
+ * 2 -> control speed only (gliders)
  *
  * @min 0.0
  * @max 2.0