diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5ab67745f1..d14a509b95 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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); diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3ef65e186f..74f849e1fb 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -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; } } diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9f90f4dfea..5bec3880a4 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -144,6 +145,7 @@ private: uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; + uORB::Publication _airflow_groundtruth_pub{ORB_ID(airflow)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};