Fixed poll timeout bug

The SIGCONT signal was being ignored by the HRT queue thread so the
usleep was not waking up early to process new work.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/drivers/device/vdev_posix.cpp

@@ -195,7 +195,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 {
 	sem_t sem;
 	int count = 0;
-	int ret;
+	int ret = -1;
 	unsigned int i;
 
 	PX4_DEBUG("Called px4_poll timeout = %d", timeout);
@@ -222,7 +222,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 
 	if (ret >= 0)
 	{
-		if (timeout >= 0)
+		if (timeout > 0)
 		{
 			// Use a work queue task
 			work_s _hpwork;
@@ -234,7 +234,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 			// out of scope
 			hrt_work_cancel(&_hpwork);
         	}
-		else
+		else if (timeout < 0) 
 		{
 			sem_wait(&sem);
 		}

src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp

@@ -303,6 +303,11 @@ int VCDevExample::main()
 		ret = 1;
 		goto fail2;
 	}
+	PX4_INFO("TEST: 100ms TIMEOUT POLL -----------");
+	if(do_poll(fd, 0, 30, 100)) {
+		ret = 1;
+		goto fail2;
+	}
 	PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
 	if(do_poll(fd, 1000, 3, 0)) {
 		ret = 1;

src/platforms/posix/work_queue/hrt_thread.c

@@ -41,6 +41,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <stdint.h>
+#include <signal.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <queue.h>
@@ -73,6 +74,26 @@ sem_t _hrt_work_lock;
  ****************************************************************************/
 static void hrt_work_process(void);
 
+static void _sighandler(int sig_num);
+
+/****************************************************************************
+ * Name: _sighandler
+ *
+ * Description:
+ *   This is the handler for the signal to wake the queue processing thread
+ *
+ * Input parameters:
+ *   sig_num - the received signal
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+static void _sighandler(int sig_num)
+{
+	PX4_DEBUG("RECEIVED SIGNAL %d", sig_num);
+}
+
 /****************************************************************************
  * Name: work_process
  *
@@ -96,6 +117,7 @@ static void hrt_work_process()
 	uint64_t elapsed;
 	uint32_t remaining;
 	uint32_t next;
+	int ret;
 
 	/* Then process queued work.  We need to keep interrupts disabled while
 	 * we process items in the work list.
@@ -185,7 +207,8 @@ static void hrt_work_process()
 
 	/* might sleep less if a signal received and new item was queued */
 	//PX4_INFO("Sleeping for %u usec", next);
-	usleep(next);
+	ret = usleep(next);
+	//PX4_INFO("WOKE UP %d", ret);
 }
 
 /****************************************************************************
@@ -251,5 +274,11 @@ void hrt_work_queue_init(void)
 					    work_hrtthread,
 					    (char *const *)NULL);
 
+	
+#ifdef __PX4_QURT
+	signal(SIGALRM, _sighandler);
+#else
+	signal(SIGCONT, _sighandler);
+#endif
 }