fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 02:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add global trajectory setpoint message

Browse Source 
Integrate global path setpoint
This commit is contained in:
JaeyoungLim
2026-01-11 07:23:33 -08:00
parent 89113c8fff
commit 04b269eb5a
5 changed files with 42 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
+1
View File
@@ -108,6 +108,7 @@ set(msg_files
GimbalManagerSetAttitude.msg
GimbalManagerSetManualControl.msg
GimbalManagerStatus.msg
GlobalTrajectorySetpoint.msg
GpioConfig.msg
GpioIn.msg
GpioOut.msg
msg/GlobalTrajectorySetpoint.msg
+16
View File
@@ -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)
src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp
+20 -26
View File
@@ -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<double>(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();
src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp
+2 -2
View File
@@ -73,7 +73,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/global_trajectory_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -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)};
src/modules/uxrce_dds_client/dds_topics.yaml
+3
View File
@@ -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