Hrt queue fixes

src/drivers/device/vdev.cpp

@@ -476,33 +476,33 @@ VDev *VDev::getDev(const char *path)
 void VDev::showDevices()
 {
 	int i=0;
-	PX4_INFO("Devices:\n");
+	PX4_INFO("Devices:");
 	for (; i<PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
-			PX4_INFO("   %s\n", devmap[i]->name);
+			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
 }
 
 void VDev::showTopics()
 {
 	int i=0;
-	PX4_INFO("Devices:\n");
+	PX4_INFO("Devices:");
 	for (; i<PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
-			PX4_INFO("   %s\n", devmap[i]->name);
+			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
 }
 
 void VDev::showFiles()
 {
 	int i=0;
-	PX4_INFO("Files:\n");
+	PX4_INFO("Files:");
 	for (; i<PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
 				 strncmp(devmap[i]->name, "/dev/", 5) != 0) {
-			PX4_INFO("   %s\n", devmap[i]->name);
+			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
 }

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

@@ -625,18 +625,18 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 				bool want_start = (_call_accel_interval == 0);
 
 				/* convert hz to hrt interval via microseconds */
-				unsigned ticks = 1000000 / arg;
+				unsigned period = 1000000 / arg;
 
 				/* check against maximum sane rate */
-				if (ticks < 500)
+				if (period < 500)
 					return -EINVAL;
 
 				/* adjust filters */
 				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
 
 				/* update interval for next measurement */
 				/* XXX this is a bit shady, but no other way to adjust... */
-				_accel_call.period = _call_accel_interval = ticks;
+				_accel_call.period = _call_accel_interval = period;
 
 				/* if we need to start the poll state machine, do it */
 				if (want_start)
@@ -749,15 +749,17 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					bool want_start = (_call_mag_interval == 0);
 
 					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
+					unsigned period = 1000000 / arg;
 
-					/* check against maximum sane rate */
-					if (ticks < 1000)
+					/* check against maximum sane rate (1ms) */
+					if (period < 1000)
 						return -EINVAL;
 
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
-					_mag_call.period = _call_mag_interval = ticks;
+					_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)
@@ -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/drivers/airspeedsim/airspeedsim.cpp

@@ -211,7 +211,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* convert hz to tick interval via microseconds */
-					long ticks = USEC2TICK(1000000 / arg);
+					unsigned long ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
 					if (ticks < USEC2TICK(_conversion_interval))

src/platforms/posix/drivers/barosim/baro.cpp

@@ -439,10 +439,10 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 				bool want_start = (_measure_ticks == 0);
 
 				/* convert hz to tick interval via microseconds */
-				unsigned ticks = USEC2TICK(1000000 / arg);
+				unsigned long ticks = USEC2TICK(1000000 / arg);
 
 				/* check against maximum rate */
-				if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL))
+				if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL))
 					return -EINVAL;
 
 				/* update interval for next measurement */
@@ -559,7 +559,7 @@ BAROSIM::cycle()
 		 * doing pressure measurements at something close to the desired rate.
 		 */
 		if ((_measure_phase != 0) &&
-		    ((long)_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) {
+		    (_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) {
 
 			/* schedule a fresh cycle call when we are ready to measure again */
 			work_queue(HPWORK,

src/platforms/posix/px4_layer/drv_hrt.c

@@ -42,7 +42,8 @@
 #include <semaphore.h>
 #include <time.h>
 #include <string.h>
-#include <stdio.h>
+#include <inttypes.h>
+#include "hrt_work.h"
 
 static struct sq_queue_s	callout_queue;
 
@@ -54,9 +55,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 +190,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 +211,7 @@ hrt_call_enter(struct hrt_call *entry)
 		} while ((call = next) != NULL);
 	}
 
-	//lldbg("scheduled\n");
+	//PX4_INFO("scheduled");
 }
 
 /**
@@ -222,7 +223,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 +244,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 +267,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 +301,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: %" PRIu64, interval);
+	}
 	entry->deadline = deadline;
 	entry->period = interval;
 	entry->callout = callout;
@@ -372,7 +379,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 +392,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 +405,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);