diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 0507e65425..dbbceae4d5 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -331,6 +331,7 @@ private:
 	mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators);
 
 	void handle_message(mavlink_message_t *msg, bool publish);
+	void handle_message_distance_sensor(const mavlink_message_t *msg);
 	void handle_message_landing_target(const mavlink_message_t *msg);
 
 	void parameters_update(bool force);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 1583865288..2452cd54b8 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -368,9 +368,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		break;
 
 	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
-		mavlink_distance_sensor_t dist;
-		mavlink_msg_distance_sensor_decode(msg, &dist);
-		publish_distance_topic(&dist);
+		handle_message_distance_sensor(msg);
 		break;
 
 	case MAVLINK_MSG_ID_HIL_GPS:
@@ -499,6 +497,13 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 
 }
 
+void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
+{
+	mavlink_distance_sensor_t dist;
+	mavlink_msg_distance_sensor_decode(msg, &dist);
+	publish_distance_topic(&dist);
+}
+
 void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
 {
 	mavlink_landing_target_t landing_target_mavlink;