diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index ea82c74d60..b9e6d2e703 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/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: | [RWTO_NUDGE](../advanced_config/parameter_reference.md#RWTO_NUDGE) | Enable wheel controller nudging while on the runway | | [FW_WING_SPAN](../advanced_config/parameter_reference.md#FW_WING_SPAN) | The wingspan of the vehicle. Used to prevent wingstrikes. | | [FW_WING_HEIGHT](../advanced_config/parameter_reference.md#FW_WING_HEIGHT) | The height of the wings above ground (ground clearance). Used to prevent wingstrikes. | -| [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 diff --git a/msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg b/msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg new file mode 100644 index 0000000000..28aa780699 --- /dev/null +++ b/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 diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 7564a9f83d..21b128c03c 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/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" diff --git a/msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h b/msg/translation_node/translations/translation_vehicle_attitude_setpoint_v1.h new file mode 100644 index 0000000000..b30e469dff --- /dev/null +++ b/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 +#include + +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); diff --git a/msg/versioned/VehicleAttitudeSetpoint.msg b/msg/versioned/VehicleAttitudeSetpoint.msg index 28aa780699..b32b336edb 100644 --- a/msg/versioned/VehicleAttitudeSetpoint.msg +++ b/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 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9057d792b9..c9a6d921fc 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/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 - || _landed + if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) { _rates_sp.reset_integral = true; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 6c902878e1..43432a4a3a 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -132,7 +132,6 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_airspd_trim, (ParamBool) _param_fw_use_airspd, diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp index 7cfc23b438..498a5332b1 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.cpp +++ b/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() -{ - // keep controlling yaw directly until we start navigation - return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; -} - -float RunwayTakeoff::getPitch(float external_pitch_setpoint) +float RunwayTakeoff::getPitch() { 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) -{ - 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 RunwayTakeoff::getThrottle(const float idle_throttle) 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) { diff --git a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h index 35e463077e..22fa8a51ec 100644 --- a/src/modules/fw_pos_control/runway_takeoff/RunwayTakeoff.h +++ b/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); - - /** - * @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); + float getRoll(); /** * @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) param_rwto_tkoff_, - (ParamInt) param_rwto_hdg_, (ParamFloat) param_rwto_max_thr_, (ParamFloat) param_rwto_psp_, (ParamFloat) param_rwto_ramp_time_, diff --git a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c index 766cd8f54e..e33fa9d616 100644 --- a/src/modules/fw_pos_control/runway_takeoff/runway_takeoff_params.c +++ b/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 * diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 9c1e72b1a5..253f80960f 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/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