px4_poll() on POSIX: Block execution in simulation if required

src/drivers/device/vdev_posix.cpp

@@ -53,6 +53,9 @@
 using namespace device;
 
 pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
+px4_sem_t lockstep_sem;
+bool sim_lockstep = false;
+bool sim_delay = false;
 
 extern "C" {
 
@@ -270,6 +273,10 @@ extern "C" {
 
 #endif
 
+		while (sim_delay) {
+			usleep(100);
+		}
+
 		PX4_DEBUG("Called px4_poll timeout = %d", timeout);
 		px4_sem_init(&sem, 0, 0);
 
@@ -398,6 +405,23 @@ extern "C" {
 		VDev::showFiles();
 	}
 
+	void px4_enable_sim_lockstep()
+	{
+		px4_sem_init(&lockstep_sem, 0, 0);
+		sim_lockstep = true;
+		sim_delay = false;
+	}
+
+	void px4_sim_start_delay()
+	{
+		sim_delay = true;
+	}
+
+	void px4_sim_stop_delay()
+	{
+		sim_delay = false;
+	}
+
 	const char *px4_get_device_names(unsigned int *handle)
 	{
 		return VDev::devList(handle);

src/platforms/px4_posix.h

@@ -146,6 +146,10 @@ __EXPORT int		px4_fsync(int fd);
 __EXPORT int		px4_access(const char *pathname, int mode);
 __EXPORT unsigned long	px4_getpid(void);
 
+__EXPORT void		px4_enable_sim_lockstep(void);
+__EXPORT void		px4_sim_start_delay(void);
+__EXPORT void		px4_sim_stop_delay(void);
+
 __END_DECLS
 #else
 #error "No TARGET OS Provided"