HITL: remove unused airspeed subsciption from gzsim

HITL: add support for actuator test
tiiuae · Aug 21, 2024 · b925819 · b925819
1 parent 78ff8c8
commit b925819
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 12 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -40,7 +40,6 @@
  * @author Thomas Gubler <[email protected]>
  */
 
-#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
@@ -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
@@ -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;