Posix: fixed time scaling for work queues

In work queues, delay is in ticks. Needed to check elapsed time
in ticks not in milliseconds.

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

src/platforms/posix/px4_layer/work_thread.c

@@ -85,13 +85,16 @@ struct wqueue_s g_work[NWORKERS];
  *
  ****************************************************************************/
 
+#define USEC_TO_TICKS(x) (uint32_t)((x)/1000ull)
+#define TICKS_TO_USEC(x) ((x)*1000ull)
+
 static void work_process(FAR struct wqueue_s *wqueue)
 {
   volatile FAR struct work_s *work;
   worker_t  worker;
   //irqstate_t flags;
   FAR void *arg;
-  uint32_t elapsed;
+  uint64_t elapsed;
   uint32_t remaining;
   uint32_t next;
 
@@ -110,8 +113,8 @@ static void work_process(FAR struct wqueue_s *wqueue)
        * zero.  Therefore a delay of zero will always execute immediately.
        */
 
-      elapsed = clock_systimer() - work->qtime;
-      //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
+      elapsed = USEC_TO_TICKS(clock_systimer() - work->qtime);
+      //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */
@@ -155,7 +158,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            */
 
           /* Here: elapsed < work->delay */
-          remaining = work->delay - elapsed;
+          remaining = TICKS_TO_USEC(work->delay - elapsed);
           if (remaining < next)
             {
               /* Yes.. Then schedule to wake up when the work is ready */
@@ -292,6 +295,7 @@ int work_usrthread(int argc, char *argv[])
 
 uint32_t clock_systimer()
 {
+	//printf("clock_systimer: %0lx\n", hrt_absolute_time());
 	return (0x00000000ffffffff & hrt_absolute_time());
 }
 #endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/posix/tests/wqueue/wqueue_test.cpp

@@ -62,14 +62,25 @@ void WQueueTest::lp_worker_cb(void *p)
 
 void WQueueTest::do_lp_work()
 {
+	static int iter = 0;
 	printf("done lp work\n");
-	_lpwork_done = true;
+	if (iter > 5)
+		_lpwork_done = true;
+	++iter;
+
+	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000);
 }
 
 void WQueueTest::do_hp_work()
 {
+	static int iter = 0;
 	printf("done hp work\n");
-	_hpwork_done = true;
+	if (iter > 5)
+		_hpwork_done = true;
+	++iter;
+
+	// requeue
+	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000);
 }
 
 int WQueueTest::main()
@@ -77,19 +88,17 @@ int WQueueTest::main()
 	appState.setRunning(true);
 
 	//Put work on HP work queue
-	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1);
+	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000);
 
 
 	//Put work on LP work queue
-	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1);
+	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000);
 
 
 	// Wait for work to finsh
 	while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) {
-		printf("  Sleeping...\n");
+		printf("  Sleeping for 2 sec...\n");
 		sleep(2);
-
-		printf("  Waiting on work...\n");
 	}
 
 	return 0;

src/platforms/px4_config.h

@@ -52,7 +52,7 @@
 #define CONFIG_ARCH_BOARD_POSIXTEST 1
 
 /** time in ms between checks for work in work queues **/
-#define CONFIG_SCHED_WORKPERIOD 10
+#define CONFIG_SCHED_WORKPERIOD 50000
 
 #define CONFIG_SCHED_INSTRUMENTATION 1
 #define CONFIG_MAX_TASKS 32