Simulator: Remove EKF2 based wait

This is cleaner and needs testing.

src/modules/simulator/simulator_mavlink.cpp

@@ -587,70 +587,28 @@ void Simulator::send()
 	fds_actuator_outputs[0].fd = _actuator_outputs_sub;
 	fds_actuator_outputs[0].events = POLLIN;
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	px4_pollfd_struct_t fds_ekf2_timestamps[1] = {};
-	fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub;
-	fds_ekf2_timestamps[0].events = POLLIN;
-
-	State state = State::WaitingForFirstEkf2Timestamp;
-#endif
-
 	while (true) {
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-
-		if (state == State::WaitingForActuatorControls) {
-#endif
-			// Wait for up to 100ms for data.
-			int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
+		// Wait for up to 100ms for data.
+		int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
 
-			if (pret == 0) {
-				// Timed out, try again.
-				continue;
-			}
-
-			if (pret < 0) {
-				PX4_ERR("poll error %s", strerror(errno));
-				continue;
-			}
-
-			if (fds_actuator_outputs[0].revents & POLLIN) {
-				// Got new data to read, update all topics.
-				parameters_update(false);
-				_vehicle_status_sub.update(&_vehicle_status);
-				send_controls();
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-				state = State::WaitingForEkf2Timestamp;
-#endif
-			}
-
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		if (pret == 0) {
+			// Timed out, try again.
+			continue;
 		}
 
-#endif
-
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-
-		if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) {
-			int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100);
-
-			if (pret == 0) {
-				// Timed out, try again.
-				continue;
-			}
-
-			if (pret < 0) {
-				PX4_ERR("poll error %s", strerror(errno));
-				continue;
-			}
+		if (pret < 0) {
+			PX4_ERR("poll error %s", strerror(errno));
+			continue;
+		}
 
-			if (fds_ekf2_timestamps[0].revents & POLLIN) {
-				orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, nullptr);
-				state = State::WaitingForActuatorControls;
-			}
+		if (fds_actuator_outputs[0].revents & POLLIN) {
+			// Got new data to read, update all topics.
+			parameters_update(false);
+			_vehicle_status_sub.update(&_vehicle_status);
+			send_controls();
 		}
 
-#endif
 	}
 }
 
@@ -743,7 +701,7 @@ void Simulator::run()
 
 			} else {
 				::close(_fd);
-				system_sleep(1);
+				system_usleep(100);
 			}
 		}
 
@@ -794,10 +752,6 @@ void Simulator::run()
 	// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
 
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	_ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps));
-#endif
-
 	// got data from simulator, now activate the sending thread
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
 	pthread_attr_destroy(&sender_thread_attr);
@@ -860,9 +814,6 @@ void Simulator::run()
 	}
 
 	orb_unsubscribe(_actuator_outputs_sub);
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	orb_unsubscribe(_ekf2_timestamps_sub);
-#endif
 }
 
 #ifdef ENABLE_UART_RC_INPUT