VehicleAttitudeSetpoint.msg: remove reset_integral and fw_control_yaw_wheel

Signed-off-by: Silvan <silvan@auterion.com>

docs/en/flight_modes_fw/takeoff.md

@@ -101,7 +101,7 @@ The _runway takeoff mode_ has the following phases:
 
 ::: info
 For a smooth takeoff, the runway wheel controller possibly needs to be tuned.
-It consists of a rate controller (P-I-FF-controller with the parameters [FW_WR_P](../advanced_config/parameter_reference.md#FW_WR_P), [FW_WR_I](../advanced_config/parameter_reference.md#FW_WR_I), [FW_WR_FF](../advanced_config/parameter_reference.md#FW_WR_FF)) and an outer loop that calculates heading setpoints from course errors and can be tuned via [RWTO_NPFG_PERIOD](#RWTO_NPFG_PERIOD).
+It consists of a rate controller (P-I-FF-controller with the parameters [FW_WR_P](../advanced_config/parameter_reference.md#FW_WR_P), [FW_WR_I](../advanced_config/parameter_reference.md#FW_WR_I), [FW_WR_FF](../advanced_config/parameter_reference.md#FW_WR_FF)).
 :::
 
 ### Parameters (Runway Takeoff)
@@ -120,7 +120,6 @@ Runway takeoff is affected by the following parameters:
 | <a id="RWTO_NUDGE"></a>[RWTO_NUDGE](../advanced_config/parameter_reference.md#RWTO_NUDGE)                   | Enable wheel controller nudging while on the runway                                                                          |
 | <a id="FW_WING_SPAN"></a>[FW_WING_SPAN](../advanced_config/parameter_reference.md#FW_WING_SPAN)             | The wingspan of the vehicle. Used to prevent wingstrikes.                                                                    |
 | <a id="FW_WING_HEIGHT"></a>[FW_WING_HEIGHT](../advanced_config/parameter_reference.md#FW_WING_HEIGHT)       | The height of the wings above ground (ground clearance). Used to prevent wingstrikes.                                        |
-| <a id="RWTO_NPFG_PERIOD"></a>[RWTO_NPFG_PERIOD](../advanced_config/parameter_reference.md#RWTO_NPFG_PERIOD) | L1 period while steering on runway. Increase for less aggressive response to course errors.                                  |
 
 ## See Also
 

msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg

@@ -0,0 +1,18 @@
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp		# time since system start (microseconds)
+
+float32 yaw_sp_move_rate	# rad/s (commanded by user)
+
+# For quaternion-based attitude control
+float32[4] q_d			# Desired quaternion for quaternion control
+
+# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
+# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
+float32[3] thrust_body		# Normalized thrust command in body FRD frame [-1,1]
+
+bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)
+
+bool fw_control_yaw_wheel	# control heading with steering wheel (used for auto takeoff on runway)
+
+# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint

msg/translation_node/translations/all_translations.h

@@ -12,3 +12,4 @@
 
 #include "translation_vehicle_status_v1.h"
 #include "translation_airspeed_validated_v1.h"
+#include "translation_vehicle_attitude_setpoint_v1.h"

msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h

@@ -0,0 +1,53 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate VehicleAttitudeSetpoint v0 <--> v1
+#include <px4_msgs_old/msg/vehicle_attitude_setpoint_v0.hpp>
+#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
+
+class VehicleAttitudeSetpointV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::VehicleAttitudeSetpointV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::VehicleAttitudeSetpoint;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/in/vehicle_attitude_setpoint";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		// Set msg_newer from msg_older
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.yaw_sp_move_rate = msg_older.yaw_sp_move_rate;
+		msg_newer.q_d[0] = msg_older.q_d[0];
+		msg_newer.q_d[1] = msg_older.q_d[1];
+		msg_newer.q_d[2] = msg_older.q_d[2];
+		msg_newer.q_d[3] = msg_older.q_d[3];
+		msg_newer.thrust_body[0] = msg_older.thrust_body[0];
+		msg_newer.thrust_body[1] = msg_older.thrust_body[1];
+		msg_newer.thrust_body[2] = msg_older.thrust_body[2];
+
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		// Set msg_older from msg_newer
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.yaw_sp_move_rate = msg_newer.yaw_sp_move_rate;
+		msg_older.q_d[0] = msg_newer.q_d[0];
+		msg_older.q_d[1] = msg_newer.q_d[1];
+		msg_older.q_d[2] = msg_newer.q_d[2];
+		msg_older.q_d[3] = msg_newer.q_d[3];
+		msg_older.thrust_body[0] = msg_newer.thrust_body[0];
+		msg_older.thrust_body[1] = msg_newer.thrust_body[1];
+		msg_older.thrust_body[2] = msg_newer.thrust_body[2];
+
+		msg_older.reset_integral = false;
+		msg_older.fw_control_yaw_wheel = false;
+
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleAttitudeSetpointV1Translation);

msg/versioned/VehicleAttitudeSetpoint.msg

@@ -1,4 +1,4 @@
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp		# time since system start (microseconds)
 
@@ -11,8 +11,4 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body FRD frame [-1,1]
 
-bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)
-
-bool fw_control_yaw_wheel	# control heading with steering wheel (used for auto takeoff on runway)
-
 # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -108,8 +108,6 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 				const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 				q.copyTo(_att_sp.q_d);
 
-				_att_sp.reset_integral = false;
-
 				_att_sp.timestamp = hrt_absolute_time();
 
 				_attitude_sp_pub.publish(_att_sp);
@@ -271,7 +269,8 @@ void FixedwingAttitudeControl::Run()
 
 		bool wheel_control = false;
 
-		if (_param_fw_w_en.get() && _att_sp.fw_control_yaw_wheel && _vcontrol_mode.flag_control_auto_enabled) {
+		// TODO listen to a runway_takeoff_status to determine when to control wheel
+		if (_param_fw_w_en.get() && _vcontrol_mode.flag_control_auto_enabled) {
 			wheel_control = true;
 		}
 
@@ -283,11 +282,10 @@ void FixedwingAttitudeControl::Run()
 
 		if (_vcontrol_mode.flag_control_rates_enabled) {
 
-			/* Reset integrators if commanded by attitude setpoint, or the aircraft is on ground
+			/* Reset integrators if the aircraft is on ground
 			 * or a multicopter (but not transitioning VTOL or tailsitter)
 			 */
-			if (_att_sp.reset_integral
+			if (_landed
-			    || _landed
 			    || !_in_fw_or_transition_wo_tailsitter_transition) {
 
 				_rates_sp.reset_integral = true;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -132,7 +132,6 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
-		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
 		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp

@@ -51,10 +51,8 @@ using namespace time_literals;
 namespace runwaytakeoff
 {
 
-void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global)
+void RunwayTakeoff::init(const hrt_abstime &time_now)
 {
-	initial_yaw_ = initial_yaw;
-	start_pos_global_ = start_pos_global;
 	takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
 	initialized_ = true;
 	time_initialized_ = time_now;
@@ -99,42 +97,26 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs
 	}
 }
 
-bool RunwayTakeoff::controlYaw()
+float RunwayTakeoff::getPitch()
-{
-	// keep controlling yaw directly until we start navigation
-	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
-}
-
-float RunwayTakeoff::getPitch(float external_pitch_setpoint)
 {
 	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
 		return math::radians(param_rwto_psp_.get());
 	}
 
-	return external_pitch_setpoint;
+	return NAN;
 }
 
-float RunwayTakeoff::getRoll(float external_roll_setpoint)
+float RunwayTakeoff::getRoll()
 {
 	// until we have enough ground clearance, set roll to 0
 	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
 		return 0.0f;
 	}
 
-	return external_roll_setpoint;
+	return NAN;
 }
 
-float RunwayTakeoff::getYaw(float external_yaw_setpoint)
+float RunwayTakeoff::getThrottle(const float idle_throttle) const
-{
-	if (param_rwto_hdg_.get() == 0 && takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
-		return initial_yaw_;
-
-	} else {
-		return external_yaw_setpoint;
-	}
-}
-
-float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
 {
 	float throttle = idle_throttle;
 
@@ -147,30 +129,18 @@ float RunwayTakeoff::getThrottle(const float idle_throttle, const float external
 		break;
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+	case RunwayTakeoffState::CLIMBOUT:
 		throttle = param_rwto_max_thr_.get();
 
 		break;
 
-	case RunwayTakeoffState::CLIMBOUT:
-		// ramp in throttle setpoint over takeoff rotation transition time
-		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
-				param_rwto_rot_time_.get());
-
-		break;
-
 	case RunwayTakeoffState::FLY:
-		throttle = external_throttle_setpoint;
+		throttle = NAN;
 	}
 
 	return throttle;
 }
 
-bool RunwayTakeoff::resetIntegrators()
-{
-	// reset integrators if we're still on runway
-	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
-}
-
 float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const
 {
 	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {

src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h

@@ -70,10 +70,8 @@ public:
 	 * @brief Initializes the state machine.
 	 *
 	 * @param time_now Absolute time since system boot [us]
-	 * @param initial_yaw Vehicle yaw angle at time of initialization [us]
-	 * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg]
 	 */
-	void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global);
+	void init(const hrt_abstime &time_now);
 
 	/**
 	 * @brief Updates the state machine based on the current vehicle condition.
@@ -103,37 +101,14 @@ public:
 	bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); }
 
 	/**
-	 * @return Initial vehicle yaw angle [rad]
-	 */
-	float getInitYaw() { return initial_yaw_; }
-
-	/**
-	 * @return The vehicle should control yaw via rudder or nose gear
-	 */
-	bool controlYaw();
-
-	/**
-	 * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
 	 * @return Pitch angle setpoint (limited while plane is on runway) [rad]
 	 */
-	float getPitch(float external_pitch_setpoint);
+	float getPitch();
 
 	/**
-	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from path navigation) [rad]
 	 * @return Roll angle setpoint [rad]
 	 */
-	float getRoll(float external_roll_setpoint);
+	float getRoll();
-
-	/**
-	 * @brief Returns the appropriate yaw angle setpoint.
-	 *
-	 * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the runway.
-	 * When it has enough ground clearance we start navigation towards WP.
-	 *
-	 * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad]
-	 * @return Yaw angle setpoint [rad]
-	 */
-	float getYaw(float external_yaw_setpoint);
 
 	/**
 	 * @brief Returns the throttle setpoint.
@@ -142,10 +117,9 @@ public:
 	 * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time
 	 *
 	 * @param idle_throttle normalized [0,1]
-	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
 	 * @return Throttle setpoint, normalized [0,1]
 	 */
-	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
+	float getThrottle(const float idle_throttle) const;
 
 	/**
 	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]
@@ -160,20 +134,9 @@ public:
 	 */
 	float getMaxPitch(const float max_pitch) const;
 
-	/**
-	 * @return Runway takeoff starting position in global frame (lat, lon) [deg]
-	 */
-	const matrix::Vector2d &getStartPosition() const { return start_pos_global_; };
-
 	// NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air
 	void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; }
 
-	/**
-	 * @return If the attitude / rate control integrators should be continually reset.
-	 * This is the case during ground roll.
-	 */
-	bool resetIntegrators();
-
 	/**
 	 * @brief Reset the state machine.
 	 */
@@ -212,22 +175,8 @@ private:
 	 */
 	hrt_abstime takeoff_time_{0};
 
-	/**
-	 * Initial yaw of the vehicle on first pass through the runway takeoff state machine.
-	 * used for heading hold mode. [rad]
-	 */
-	float initial_yaw_{0.f};
-
-	/**
-	 * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The
-	 * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The
-	 * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg]
-	 */
-	matrix::Vector2d start_pos_global_{};
-
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::RWTO_TKOFF>) param_rwto_tkoff_,
-		(ParamInt<px4::params::RWTO_HDG>) param_rwto_hdg_,
 		(ParamFloat<px4::params::RWTO_MAX_THR>) param_rwto_max_thr_,
 		(ParamFloat<px4::params::RWTO_PSP>) param_rwto_psp_,
 		(ParamFloat<px4::params::RWTO_RAMP_TIME>) param_rwto_ramp_time_,

src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c

@@ -47,21 +47,6 @@
  */
 PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
 
-/**
- * Specifies which heading should be held during the runway takeoff ground roll.
- *
- * 0: airframe heading when takeoff is initiated
- * 1: position control along runway direction (bearing defined from vehicle position on takeoff initiation to MAV_CMD_TAKEOFF
- *    position defined by operator)
- *
- * @value 0 Airframe
- * @value 1 Runway
- * @min 0
- * @max 1
- * @group Runway Takeoff
- */
-PARAM_DEFINE_INT32(RWTO_HDG, 0);
-
 /**
  * Max throttle during runway takeoff.
  *
@@ -102,18 +87,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
  */
 PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);
 
-/**
- * NPFG period while steering on runway
- *
- * @unit s
- * @min 1.0
- * @max 100.0
- * @decimal 1
- * @increment 0.1
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_NPFG_PERIOD, 5.0f);
-
 /**
  * Enable use of yaw stick for nudging the wheel during runway ground roll
  *

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -63,8 +63,6 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
-	EXPECT_EQ(attitude.reset_integral, false);
-	EXPECT_EQ(attitude.fw_control_yaw_wheel, false);
 }
 
 class PositionControlBasicTest : public ::testing::Test