fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-08 11:58:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Log airflow topics

Browse Source
This commit is contained in:
JaeyoungLim
2025-11-23 07:22:33 -08:00
parent 9c68cdb62f
commit e7bf89136d
3 changed files with 20 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/logger/logged_topics.cpp
+2
View File
@@ -155,6 +155,7 @@ void LoggedTopics::add_default_topics()
add_topic("fixed_wing_longitudinal_setpoint");
add_topic("longitudinal_control_configuration");
add_topic("lateral_control_configuration");
add_topic("airflow");
add_optional_topic("fixed_wing_lateral_guidance_status", 100);
add_optional_topic("fixed_wing_lateral_status", 100);
add_optional_topic("fixed_wing_runway_control", 100);
@@ -222,6 +223,7 @@ void LoggedTopics::add_default_topics()
param_get(param_find("SYS_HITL"), &sys_hitl);
if (sys_hitl >= 1) {
add_topic("airflow_groundtruth", 10);
add_topic("vehicle_angular_velocity_groundtruth", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);
src/modules/simulation/gz_bridge/GZBridge.cpp
+16
View File
@@ -582,6 +582,22 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg)
local_position_groundtruth.timestamp = timestamp;
_lpos_ground_truth_pub.publish(local_position_groundtruth);
airflow_s airflow_groundtruth;
airflow_groundtruth.timestamp = timestamp;
// Body-relateive air velocity
matrix::Dcmf Rib(matrix::Quatf{(float)q_nb.W(), (float)q_nb.X(), (float)q_nb.Y(), (float)q_nb.Z()});
matrix::Vector3f velocity_f = matrix::Vector3f{(float)velocity(0), (float)velocity(1), (float)velocity(2)};
matrix::Vector3f body_velocity = Rib.transpose() * velocity_f;
airflow_groundtruth.u = body_velocity(0);
airflow_groundtruth.v = body_velocity(1);
airflow_groundtruth.w = body_velocity(2);
///TODO: Include wind
airflow_groundtruth.windspeed_north = 0.0;
airflow_groundtruth.windspeed_east = 0.0;
airflow_groundtruth.windspeed_down = 0.0;
_airflow_groundtruth_pub.publish(airflow_groundtruth);
return;
}
}
src/modules/simulation/gz_bridge/GZBridge.hpp
+2
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/airflow.h>
#include <gz/math.hh>
#include <gz/msgs.hh>
@@ -144,6 +145,7 @@ private:
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
uORB::Publication<airflow_s> _airflow_groundtruth_pub{ORB_ID(airflow)};
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};