POSIX: Fixes for HRT implementation of simulated HW clock polling

There is a race condition for the accel and mag polling rates.
Whichever one gets set first, the other will be uninitialized.

Set the mag polling rate to 1ms if uninitilized.

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

src/platforms/posix/drivers/accelsim/accelsim.cpp

@@ -759,6 +759,8 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_mag_call.period = _call_mag_interval = period;
 
+					//PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval);
+
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
 						start();
@@ -934,7 +936,17 @@ ACCELSIM::start()
 	_mag_reports->flush();
 
 	/* start polling at the specified rate */
+	//PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval);
 	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this);
+
+
+	// There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but mag period is set to 0
+	if (_call_mag_interval == 0) {
+		PX4_ERR("_call_mag_interval uninitilized - would have set period delay of 0");
+		_call_mag_interval = 1000;
+	}
+
+	//PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval);
 	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this);
 }
 
@@ -948,6 +960,7 @@ ACCELSIM::stop()
 void
 ACCELSIM::measure_trampoline(void *arg)
 {
+	//PX4_INFO("ACCELSIM::measure_trampoline");
 	ACCELSIM *dev = (ACCELSIM *)arg;
 
 	/* make another measurement */
@@ -957,6 +970,7 @@ ACCELSIM::measure_trampoline(void *arg)
 void
 ACCELSIM::mag_measure_trampoline(void *arg)
 {
+	//PX4_INFO("ACCELSIM::mag_measure_trampoline");
 	ACCELSIM *dev = (ACCELSIM *)arg;
 
 	/* make another measurement */

src/platforms/posix/px4_layer/drv_hrt.c

@@ -42,7 +42,7 @@
 #include <semaphore.h>
 #include <time.h>
 #include <string.h>
-#include <stdio.h>
+#include "hrt_work_lock.h"
 
 static struct sq_queue_s	callout_queue;
 
@@ -54,9 +54,9 @@ __EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
 
 static void		hrt_call_reschedule(void);
 
-// Intervals in ms
+// Intervals in usec
 #define HRT_INTERVAL_MIN	50
-#define HRT_INTERVAL_MAX	50000
+#define HRT_INTERVAL_MAX	50000000
 
 static sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
@@ -189,12 +189,12 @@ hrt_call_enter(struct hrt_call *entry)
 {
 	struct hrt_call	*call, *next;
 
-	//printf("hrt_call_enter\n");
+	//PX4_INFO("hrt_call_enter");
 	call = (struct hrt_call *)sq_peek(&callout_queue);
 
 	if ((call == NULL) || (entry->deadline < call->deadline)) {
 		sq_addfirst(&entry->link, &callout_queue);
-		//lldbg("call enter at head, reschedule\n");
+		//if (call != NULL) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline);
 		/* we changed the next deadline, reschedule the timer event */
 		hrt_call_reschedule();
 
@@ -210,7 +210,7 @@ hrt_call_enter(struct hrt_call *entry)
 		} while ((call = next) != NULL);
 	}
 
-	//lldbg("scheduled\n");
+	//PX4_INFO("scheduled");
 }
 
 /**
@@ -222,7 +222,7 @@ static void
 hrt_tim_isr(void *p)
 {
 
-	//printf("hrt_tim_isr\n");
+	//PX4_INFO("hrt_tim_isr");
 	/* run any callouts that have met their deadline */
 	hrt_call_invoke();
 
@@ -243,11 +243,11 @@ static void
 hrt_call_reschedule()
 {
 	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
 	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
-	uint32_t	ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
 
-	//printf("hrt_call_reschedule\n");
+	//PX4_INFO("hrt_call_reschedule");
 	
 	/*
 	 * Determine what the next deadline will be.
@@ -266,26 +266,29 @@ hrt_call_reschedule()
 		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
-			ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
+			delay = HRT_INTERVAL_MIN;
 
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
-			ticks = USEC2TICK((next->deadline - now)*1000);
+			delay = next->deadline - now;
 		}
 	}
 
 	// There is no timer ISR, so simulate one by putting an event on the 
 	// high priority work queue
-	//printf("ticks = %u\n", ticks);
-        work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks);
+
+	// Remove the existing expiry and update with the new expiry
+	hrt_work_cancel(&_hrt_work);
+
+        hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, NULL, delay);
 }
 
 static void
 hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
 {
-	//printf("hrt_call_internal\n");
+	//PX4_INFO("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
 	hrt_lock();
-	//printf("hrt_call_internal after lock\n");
+	//PX4_INFO("hrt_call_internal after lock");
 	/* if the entry is currently queued, remove it */
 	/* note that we are using a potentially uninitialised
 	   entry->link here, but it is safe as sq_rem() doesn't
@@ -297,6 +300,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	if (entry->deadline != 0)
 		sq_rem(&entry->link, &callout_queue);
 
+	if (interval < HRT_INTERVAL_MIN) {
+		PX4_ERR("hrt_call_internal interval too short: %lu", interval);
+	}
 	entry->deadline = deadline;
 	entry->period = interval;
 	entry->callout = callout;
@@ -372,7 +378,7 @@ hrt_call_invoke(void)
 			break;
 
 		sq_rem(&call->link, &callout_queue);
-		//lldbg("call pop\n");
+		//PX4_INFO("call pop");
 
 		/* save the intended deadline for periodic calls */
 		deadline = call->deadline;
@@ -385,7 +391,7 @@ hrt_call_invoke(void)
 			// Unlock so we don't deadlock in callback
 			hrt_unlock();
 
-			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			//PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg);
 			call->callout(call->arg);
 
 			hrt_lock();
@@ -398,6 +404,7 @@ hrt_call_invoke(void)
 			// using hrt_call_delay()
 			if (call->deadline <= now) {
 				call->deadline = deadline + call->period;
+				//PX4_INFO("call deadline set to %lu now=%lu", call->deadline,  now);
 			}
 
 			hrt_call_enter(call);

src/platforms/posix/px4_layer/hrt_queue.c

@@ -45,6 +45,7 @@
 #include <queue.h>
 #include <stdio.h>
 #include <semaphore.h>
+#include <drivers/drv_hrt.h>
 #include <px4_workqueue.h>
 #include "hrt_work_lock.h"
 
@@ -104,7 +105,7 @@
 
 int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay)
 {
-  struct wqueue_s *wqueue = &hrt_work;
+  struct wqueue_s *wqueue = &g_hrt_work;
 
   /* First, initialize the work structure */
 
@@ -118,8 +119,8 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
    */
 
   hrt_work_lock();
-  work->qtime  = hrt_absolute_tme(); /* Time work queued */
-  PX4_INFO("work_queue adding work delay=%u time=%lu", delay, work->qtime);
+  work->qtime  = hrt_absolute_time(); /* Time work queued */
+  //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
 
   dq_addlast((dq_entry_t *)work, &wqueue->q);
   px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */

src/platforms/posix/px4_layer/hrt_thread.c

@@ -46,9 +46,7 @@
 #include <queue.h>
 #include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
-#include "work_lock.h"
-
-#ifdef CONFIG_SCHED_WORKQUEUE
+#include "hrt_work_lock.h"
 
 /****************************************************************************
  * Pre-processor Definitions
@@ -116,14 +114,14 @@ static void hrt_work_process()
        * zero.  Therefore a delay of zero will always execute immediately.
        */
 
-      elapsed = USEC2TICK(hrt_absolute_time() - work->qtime);
-      PX4_INFO("work_process: wq=%p in ticks elapsed=%lu delay=%u work=%p", wqueue, elapsed, work->delay, work);
+      elapsed = hrt_absolute_time() - work->qtime;
+      //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work);
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */
 
           (void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
-	  PX4_INFO("Dequeued work=%p", work);
+	  //PX4_INFO("Dequeued work=%p", work);
 
           /* Extract the work description from the entry (in case the work
            * instance by the re-used after it has been de-queued).
@@ -155,7 +153,6 @@ static void hrt_work_process()
 
           hrt_work_lock();
           work  = (FAR struct work_s *)wqueue->q.head;
-      	  PX4_INFO("Done work - next is %p", work);
         }
       else
         {
@@ -165,6 +162,7 @@ static void hrt_work_process()
 
           /* Here: elapsed < work->delay */
           remaining = work->delay - elapsed;
+          //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed);
           if (remaining < next)
             {
               /* Yes.. Then schedule to wake up when the work is ready */
@@ -175,6 +173,7 @@ static void hrt_work_process()
           /* Then try the next in the list. */
 
           work = (FAR struct work_s *)work->dq.flink;
+          //PX4_INFO("next %u work %p", next, work);
         }
     }
 
@@ -184,32 +183,16 @@ static void hrt_work_process()
   hrt_work_unlock();
 
   /* might sleep less if a signal received and new item was queued */
+  //PX4_INFO("Sleeping for %u usec", next);
   usleep(next);
 }
 
 /****************************************************************************
- * Public Functions
- ****************************************************************************/
-void hrt_work_queue_init(void)
-{
-	sem_init(&_hrt_work_lock, 0, 1);
-
-	// Create high priority worker thread
-	hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
-			       SCHED_DEFAULT,
-			       SCHED_PRIORITY_MAX,
-			       2000,
-			       work_hrtthread,
-			       (char* const*)NULL);
-
-}
-
-/****************************************************************************
- * Name: work_hpthread, work_lpthread, and work_usrthread
+ * Name: work_hrtthread
  *
  * Description:
- *   These are the worker threads that performs actions placed on the work
- *   lists.
+ *   This is the worker threads that performs actions placed on the ISR work
+ *   list.
  *
  *   work_hpthread and work_lpthread:  These are the kernel mode work queues
  *     (also build in the flat build).  One of these threads also performs
@@ -218,12 +201,6 @@ void hrt_work_queue_init(void)
  *
  *     These worker threads are started by the OS during normal bringup.
  *
- *   work_usrthread:  This is a user mode work queue.  It must be built into
- *     the applicatino blob during the user phase of a kernel build.  The
- *     user work thread will then automatically be started when the system
- *     boots by calling through the pointer found in the header on the user
- *     space blob.
- *
  *   All of these entrypoints are referenced by OS internally and should not
  *   not be accessed by application logic.
  *
@@ -235,7 +212,7 @@ void hrt_work_queue_init(void)
  *
  ****************************************************************************/
 
-int work_hrtthread(int argc, char *argv[])
+static int work_hrtthread(int argc, char *argv[])
 {
   /* Loop forever */
 
@@ -258,3 +235,21 @@ int work_hrtthread(int argc, char *argv[])
   return PX4_OK; /* To keep some compilers happy */
 }
 
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+void hrt_work_queue_init(void)
+{
+	sem_init(&_hrt_work_lock, 0, 1);
+
+	// Create high priority worker thread
+	g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX,
+			       2000,
+			       work_hrtthread,
+			       (char* const*)NULL);
+
+}
+

src/platforms/posix/px4_layer/hrt_work_cancel.c

+/****************************************************************************
+ * libc/wqueue/work_cancel.c
+ *
+ *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <queue.h>
+#include <px4_workqueue.h>
+#include "hrt_work_lock.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: work_cancel
+ *
+ * Description:
+ *   Cancel previously queued work.  This removes work from the work queue.
+ *   After work has been canceled, it may be re-queue by calling work_queue()
+ *   again.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID
+ *   work   - The previously queue work structure to cancel
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int hrt_work_cancel(struct work_s *work)
+{
+  struct wqueue_s *wqueue = &g_hrt_work;
+  //irqstate_t flags;
+
+  //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+
+  /* Cancelling the work is simply a matter of removing the work structure
+   * from the work queue.  This must be done with interrupts disabled because
+   * new work is typically added to the work queue from interrupt handlers.
+   */
+
+  hrt_work_lock();
+  if (work->worker != NULL)
+    {
+      /* A little test of the integrity of the work queue */
+
+      //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
+      //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
+
+      /* Remove the entry from the work queue and make sure that it is
+       * mark as availalbe (i.e., the worker field is nullified).
+       */
+
+      dq_rem((FAR dq_entry_t *)work, &wqueue->q);
+      work->worker = NULL;
+    }
+
+  hrt_work_unlock();
+  return PX4_OK;
+}

src/platforms/posix/px4_layer/hrt_work_lock.h

@@ -33,22 +33,33 @@
 
 #include <px4_log.h>
 #include <semaphore.h>
-#include <stdio.h>
+#include <px4_workqueue.h>
 
 #pragma once
+
+__BEGIN_DECLS
+
 extern sem_t _hrt_work_lock;
+extern struct wqueue_s g_hrt_work;
+
+void hrt_work_queue_init(void);
+int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay);
+int hrt_work_cancel(struct work_s *work);
 
 inline void hrt_work_lock();
+inline void hrt_work_unlock();
+
 inline void hrt_work_lock()
 {
-	PX4_INFO("hrt_work_lock");
+	//PX4_INFO("hrt_work_lock");
 	sem_wait(&_hrt_work_lock);
 }
 
-inline void hrt_work_unlock();
 inline void hrt_work_unlock()
 {
-	PX4_INFO("hrt_work_unlock");
+	//PX4_INFO("hrt_work_unlock");
 	sem_post(&_hrt_work_lock);
 }
 
+__END_DECLS
+

src/platforms/posix/px4_layer/module.mk

@@ -38,6 +38,9 @@
 SRCS		 = 	\
 			px4_posix_impl.cpp \
 			px4_posix_tasks.cpp  \
+			hrt_thread.c \
+			hrt_queue.c \
+			hrt_work_cancel.c \
 			work_thread.c \
 			work_queue.c \
 			work_cancel.c \

src/platforms/posix/px4_layer/px4_posix_impl.cpp

@@ -46,6 +46,7 @@
 #include <errno.h>
 #include <unistd.h>
 #include "systemlib/param/param.h"
+#include "hrt_work_lock.h"
 
 __BEGIN_DECLS
 
@@ -63,6 +64,7 @@ void init_once(void);
 void init_once(void)
 {
 	work_queues_init();
+	hrt_work_queue_init();
 	hrt_init();
 }
 

src/platforms/posix/px4_layer/work_cancel.c

@@ -41,6 +41,7 @@
 #include <px4_defines.h>
 #include <queue.h>
 #include <px4_workqueue.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -88,7 +89,6 @@
 int work_cancel(int qid, struct work_s *work)
 {
   struct wqueue_s *wqueue = &g_work[qid];
-  //irqstate_t flags;
 
   //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
 
@@ -97,7 +97,7 @@ int work_cancel(int qid, struct work_s *work)
    * new work is typically added to the work queue from interrupt handlers.
    */
 
-  //flags = irqsave();
+  work_lock(qid);
   if (work->worker != NULL)
     {
       /* A little test of the integrity of the work queue */
@@ -113,7 +113,7 @@ int work_cancel(int qid, struct work_s *work)
       work->worker = NULL;
     }
 
-  //irqrestore(flags);
+  work_unlock(qid);
   return PX4_OK;
 }
 

src/platforms/posix/px4_layer/work_thread.c

@@ -114,7 +114,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
        */
 
       elapsed = USEC2TICK(clock_systimer() - work->qtime);
-      //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
+      //PX4_INFO("work_process: queue=%p in ticks elapsed=%lu delay=%u time=%u", wqueue, elapsed, work->delay,clock_systimer());
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */

src/platforms/px4_workqueue.h

@@ -69,7 +69,7 @@ struct work_s
   struct dq_entry_s dq;  /* Implements a doubly linked list */
   worker_t  worker;      /* Work callback */
   void *arg;             /* Callback argument */
-  uint32_t  qtime;       /* Time work queued */
+  uint64_t  qtime;       /* Time work queued */
   uint32_t  delay;       /* Delay until work performed */
 };