From b92581949bec88045bde815d13ee9f09c5a377cf Mon Sep 17 00:00:00 2001
From: haitomatic <hai.to@unikie.com>
Date: Wed, 21 Aug 2024 09:32:42 +0000
Subject: [PATCH] HITL: remove unused airspeed subsciption from gzsim HITL: add
 support for actuator test

---
 src/modules/mavlink/mavlink_receiver.cpp              | 10 ----------
 src/modules/mavlink/mavlink_receiver.h                |  2 --
 src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 10 ++++++++++
 3 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 36958cbe69b2..3ab7055067d6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -40,7 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <lib/airspeed/airspeed.h>
 #include <lib/conversion/rotation.h>
 #include <lib/systemlib/px4_macros.h>
 
@@ -2661,15 +2660,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	const double dt = math::constrain((timestamp_sample - _hil_timestamp_prev) * 1e-6, 0.001, 0.1);
 	_hil_timestamp_prev = timestamp_sample;
 
-	/* airspeed */
-	airspeed_s airspeed{};
-	airspeed.timestamp_sample = timestamp_sample;
-	airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
-	airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
-	airspeed.air_temperature_celsius = 15.f;
-	airspeed.timestamp = hrt_absolute_time();
-	_airspeed_pub.publish(airspeed);
-
 	/* Receive attitude quaternion from gz-sim and publish vehicle attitude
 	 * groundtruth and angular velocity ground truth
 	 */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index b47ca22c564b..8c6946fdb346 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -62,7 +62,6 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
@@ -288,7 +287,6 @@ class MavlinkReceiver : public ModuleParams
 	uint16_t _mavlink_status_last_packet_rx_drop_count{0};
 
 	// ORB publications
-	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
index 8e8b6200fd98..01d317b66975 100644
--- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
+++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
@@ -37,6 +37,7 @@
 #include <lib/mixer_module/mixer_module.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/esc_status.h>
@@ -84,6 +85,7 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams
 	}
 
 	uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
+	uORB::Subscription _act_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _rover_thrust_sub;
 	uORB::Subscription _rover_torque_sub;
@@ -219,6 +221,14 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams
 				}
 			}
 
+			actuator_test_s actuator_test;
+
+			if (_act_test_sub.copy(&actuator_test)) {
+				if (actuator_test.action != 0) {
+					msg.mode |= MAV_MODE_FLAG_TEST_ENABLED;
+				}
+			}
+
 			// [custom use of flag] depending on OutputFunction, if output function is motor type, set bit in bitfield
 			uint64_t flags = 0;