From 04b269eb5a4693f320a62e3be2f0e779f92c0bc4 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sun, 11 Jan 2026 07:23:33 -0800 Subject: [PATCH] Add global trajectory setpoint message Integrate global path setpoint --- msg/CMakeLists.txt | 1 + msg/GlobalTrajectorySetpoint.msg | 16 +++++++ .../FixedWingGuidanceControl.cpp | 46 ++++++++----------- .../FixedWingGuidanceControl.hpp | 4 +- src/modules/uxrce_dds_client/dds_topics.yaml | 3 ++ 5 files changed, 42 insertions(+), 28 deletions(-) create mode 100644 msg/GlobalTrajectorySetpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 29b538401a..490dc06345 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -108,6 +108,7 @@ set(msg_files GimbalManagerSetAttitude.msg GimbalManagerSetManualControl.msg GimbalManagerStatus.msg + GlobalTrajectorySetpoint.msg GpioConfig.msg GpioIn.msg GpioOut.msg diff --git a/msg/GlobalTrajectorySetpoint.msg b/msg/GlobalTrajectorySetpoint.msg new file mode 100644 index 0000000000..4537c30bb7 --- /dev/null +++ b/msg/GlobalTrajectorySetpoint.msg @@ -0,0 +1,16 @@ +# Trajectory setpoint in NED frame +# Input to PID position controller. +# Needs to be kinematically consistent and feasible for smooth flight. +# setting a value to NaN means the state should not be controlled + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +# NED local world frame +float32[3] velocity # in meters/second +float32[3] acceleration # in meters/second^2 +float32[3] jerk # in meters/second^3 (for logging only) diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp index af74245086..4e3e08ff8c 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp @@ -338,13 +338,13 @@ FixedWingGuidanceControl::Run() } if (_control_mode.flag_control_offboard_enabled) { - trajectory_setpoint_s trajectory_setpoint; + global_trajectory_setpoint_s global_trajectory_setpoint; - if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + if (_global_trajectory_setpoint_sub.update(&global_trajectory_setpoint)) { bool valid_setpoint = false; _pos_sp_triplet = {}; // clear any existing - _pos_sp_triplet.timestamp = trajectory_setpoint.timestamp; - _pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp; + _pos_sp_triplet.timestamp = global_trajectory_setpoint.timestamp; + _pos_sp_triplet.current.timestamp = global_trajectory_setpoint.timestamp; _pos_sp_triplet.current.cruising_speed = NAN; // ignored _pos_sp_triplet.current.cruising_throttle = NAN; // ignored _pos_sp_triplet.current.vx = NAN; @@ -354,31 +354,26 @@ FixedWingGuidanceControl::Run() _pos_sp_triplet.current.lon = static_cast(NAN); _pos_sp_triplet.current.alt = NAN; - if (Vector3f(trajectory_setpoint.position).isAllFinite()) { - if (_global_local_proj_ref.isInitialized()) { - double lat; - double lon; - _global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon); - valid_setpoint = true; - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _pos_sp_triplet.current.lat = lat; - _pos_sp_triplet.current.lon = lon; - _pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2]; - } - - } - - if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) { + if (PX4_ISFINITE(global_trajectory_setpoint.lat) && PX4_ISFINITE(global_trajectory_setpoint.lon) + && PX4_ISFINITE(global_trajectory_setpoint.alt)) { valid_setpoint = true; _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - _pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0]; - _pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1]; - _pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2]; + _pos_sp_triplet.current.lat = global_trajectory_setpoint.lat; + _pos_sp_triplet.current.lon = global_trajectory_setpoint.lon; + _pos_sp_triplet.current.alt = global_trajectory_setpoint.alt; + } - if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) { - Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + if (Vector3f(global_trajectory_setpoint.velocity).isAllFinite()) { + valid_setpoint = true; + _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _pos_sp_triplet.current.vx = global_trajectory_setpoint.velocity[0]; + _pos_sp_triplet.current.vy = global_trajectory_setpoint.velocity[1]; + _pos_sp_triplet.current.vz = global_trajectory_setpoint.velocity[2]; + + if (Vector3f(global_trajectory_setpoint.acceleration).isAllFinite()) { + Vector2f velocity_sp_2d(global_trajectory_setpoint.velocity[0], global_trajectory_setpoint.velocity[1]); Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized(); - Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]); + Vector2f acceleration_sp_2d(global_trajectory_setpoint.acceleration[0], global_trajectory_setpoint.acceleration[1]); Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) * normalized_velocity_sp_2d; float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized()); @@ -392,7 +387,6 @@ FixedWingGuidanceControl::Run() _position_setpoint_current_valid = valid_setpoint; } - } airspeed_poll(); diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp index 3beece5a80..ed99147fce 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -170,7 +170,7 @@ private: uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _global_trajectory_setpoint_sub{ORB_ID(global_trajectory_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index dc6cc1f2cf..adeb4dba46 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -160,6 +160,9 @@ subscriptions: - topic: /fmu/in/trajectory_setpoint type: px4_msgs::msg::TrajectorySetpoint + - topic: /fmu/in/global_trajectory_setpoint + type: px4_msgs::msg::GlobalTrajectorySetpoint + - topic: /fmu/in/vehicle_attitude_setpoint type: px4_msgs::msg::VehicleAttitudeSetpoint