diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 84fc92e7f0..7e66cb71b2 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -344,7 +344,7 @@ private:
 	void send_controls();
 	void send_heartbeat();
 	void send_mavlink_message(const mavlink_message_t &aMsg);
-	void set_publish(const bool publish);
+	void set_publish(const bool publish = false);
 	void update_sensors(mavlink_hil_sensor_t *imu);
 	void update_gps(mavlink_hil_gps_t *gps_sim);
 
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 658811f51c..c3fe6bbb74 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -837,8 +837,10 @@ void Simulator::poll_for_MAVLink_messages()
 				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
+						bool prev_publish = _publish;
 						set_publish(true);
 						handle_message(&msg);
+						set_publish(prev_publish);
 					}
 				}
 			}
@@ -846,8 +848,6 @@ void Simulator::poll_for_MAVLink_messages()
 
 #endif
 	}
-
-	set_publish(false);
 }