diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index a74cce6e58..21cc7859da 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -276,7 +276,7 @@ private:
 
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
-	int publish_odometry_topic(mavlink_message_t *odom_mavlink);
+	int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
 	int publish_distance_topic(mavlink_distance_sensor_t *dist);
 
 	static Simulator *_instance;
@@ -338,6 +338,7 @@ private:
 	void handle_message_landing_target(const mavlink_message_t *msg);
 	void handle_message_optical_flow(const mavlink_message_t *msg);
 	void handle_message_rc_channels(const mavlink_message_t *msg);
+	void handle_message_vision_position_estimate(const mavlink_message_t *msg);
 
 	void parameters_update(bool force);
 	void poll_topics();
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 92bf3b69d2..11ff5d5efb 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -302,7 +302,7 @@ void Simulator::handle_message(mavlink_message_t *msg)
 
 	case MAVLINK_MSG_ID_ODOMETRY:
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
-		publish_odometry_topic(msg);
+		handle_message_vision_position_estimate(msg);
 		break;
 
 	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
@@ -542,6 +542,11 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
 	}
 }
 
+void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
+{
+	publish_odometry_topic(msg);
+}
+
 void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
 {
 	uint8_t  buf[MAVLINK_MAX_PACKET_LEN];
@@ -1100,7 +1105,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 	return OK;
 }
 
-int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
+int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
 {
 	uint64_t timestamp = hrt_absolute_time();