diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index 79eb44ba8d..e1042c6470 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -163,12 +163,13 @@ int Simulator::start(int argc, char *argv[])
 			_instance->initialize_sensor_data();
 #ifndef __PX4_QURT
 			// Update sensor data
-			_instance->poll_for_MAVLink_messages(false);
+			_instance->poll_for_MAVLink_messages();
 #endif
 
 		} else if (argv[2][1] == 'p') {
 			// Update sensor data
-			_instance->poll_for_MAVLink_messages(true);
+			_instance->set_publish(true);
+			_instance->poll_for_MAVLink_messages();
 
 		} else {
 			_instance->initialize_sensor_data();
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 817c87e132..84fc92e7f0 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -330,7 +330,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(mavlink_message_t *msg);
 	void handle_message_distance_sensor(const mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(const mavlink_message_t *msg);
 	void handle_message_landing_target(const mavlink_message_t *msg);
@@ -338,12 +338,13 @@ private:
 
 	void parameters_update(bool force);
 	void poll_topics();
-	void poll_for_MAVLink_messages(bool publish);
+	void poll_for_MAVLink_messages();
 	void request_hil_state_quaternion();
 	void send();
 	void send_controls();
 	void send_heartbeat();
 	void send_mavlink_message(const mavlink_message_t &aMsg);
+	void set_publish(const bool publish);
 	void update_sensors(mavlink_hil_sensor_t *imu);
 	void update_gps(mavlink_hil_gps_t *gps_sim);
 
@@ -365,6 +366,8 @@ private:
 	struct map_projection_reference_s _hil_local_proj_ref {};
 
 	bool _hil_local_proj_inited{false};
+	bool _publish{false};
+
 	double _hil_ref_lat{0};
 	double _hil_ref_lon{0};
 	float _hil_ref_alt{0.0f};
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index f34f289c9a..658811f51c 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -289,7 +289,7 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
 	write_gps_data((void *)&gps);
 }
 
-void Simulator::handle_message(mavlink_message_t *msg, bool publish)
+void Simulator::handle_message(mavlink_message_t *msg)
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_HIL_SENSOR: {
@@ -318,7 +318,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			last_time = now_us;
 #endif
 
-			if (publish) {
+			if (_publish) {
 				publish_sensor_topics(&imu);
 			}
 
@@ -373,7 +373,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
 
-		if (publish) {
+		if (_publish) {
 			//PX4_WARN("FIXME:  Need to publish GPS topic.  Not done yet.");
 		}
 
@@ -386,7 +386,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		fill_rc_input_msg(&_rc_input, &rc_channels);
 
 		// publish message
-		if (publish) {
+		if (_publish) {
 			int rc_multi;
 			orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH);
 		}
@@ -402,7 +402,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		handle_message_hil_state_quaternion(msg);
 		break;
 	}
-
 }
 
 void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
@@ -664,7 +663,7 @@ void Simulator::initialize_sensor_data()
 	write_airspeed_data(&airspeed);
 }
 
-void Simulator::poll_for_MAVLink_messages(bool publish)
+void Simulator::poll_for_MAVLink_messages()
 {
 #ifdef __PX4_DARWIN
 	pthread_setname_np("sim_rcv");
@@ -818,7 +817,7 @@ void Simulator::poll_for_MAVLink_messages(bool publish)
 
 				for (int i = 0; i < len; i++) {
 					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &mavlink_status)) {
-						handle_message(&msg, publish);
+						handle_message(&msg);
 					}
 				}
 			}
@@ -838,7 +837,8 @@ void Simulator::poll_for_MAVLink_messages(bool publish)
 				for (int i = 0; i < len; ++i) {
 					if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
 						// have a message, handle it
-						handle_message(&msg, true);
+						set_publish(true);
+						handle_message(&msg);
 					}
 				}
 			}
@@ -846,6 +846,8 @@ void Simulator::poll_for_MAVLink_messages(bool publish)
 
 #endif
 	}
+
+	set_publish(false);
 }
 
 
@@ -1214,3 +1216,8 @@ int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
 
 	return OK;
 }
+
+void Simulator::set_publish(const bool publish)
+{
+	_publish = publish;
+}