Merge branch 'master' of github.com:PX4/Firmware into px4dev_new_param

2 files changed, 45 insertions, 4 deletions

diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index bc5d7b36b..1a0d7bd45 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -111,8 +111,8 @@ class uploader(object):
 	READ_MULTI_MAX	= 60		# protocol max is 255, something overflows with >= 64
 
 	def __init__(self, portname, baudrate):
-		# open the port
-		self.port = serial.Serial(portname, baudrate, timeout=10)
+		# open the port, keep the default timeout short so we can poll quickly
+		self.port = serial.Serial(portname, baudrate, timeout=0.25)
 
 	def close(self):
 		if self.port is not None:
@@ -171,7 +171,11 @@ class uploader(object):
 	def __erase(self):
 		self.__send(uploader.CHIP_ERASE 
 				+ uploader.EOC)
+		# erase is very slow, give it 10s
+		old_timeout = self.port.timeout
+		self.port.timeout = 10
 		self.__getSync()
+		self.port.timeout = old_timeout
 
 	# send a PROG_MULTI command to write a collection of bytes
 	def __program_multi(self, data):
diff --git a/nuttx/configs/px4fmu/src/up_hrt.c b/nuttx/configs/px4fmu/src/up_hrt.c
index 507358f8b..3792d0d21 100644
--- a/nuttx/configs/px4fmu/src/up_hrt.c
+++ b/nuttx/configs/px4fmu/src/up_hrt.c
@@ -244,9 +244,21 @@
  */
 static struct sq_queue_s	callout_queue;
 
+/* latency baseline (last compare value applied) */
+static uint16_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint16_t			latency_actual;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT	8
+static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+
 /* timer-specific functions */
 static void		hrt_tim_init(void);
 static int		hrt_tim_isr(int irq, void *context);
+static void		hrt_latency_update(void);
 
 /* callout list manipulation */
 static void		hrt_call_internal(struct hrt_call *entry,
@@ -502,6 +514,9 @@ hrt_tim_isr(int irq, void *context)
 {
 	uint32_t status;
 
+	/* grab the timer for latency tracking purposes */
+	latency_actual = rCNT;
+
 	/* copy interrupt status */
 	status = rSR;
 
@@ -516,6 +531,10 @@ hrt_tim_isr(int irq, void *context)
 
 	/* was this a timer tick? */
 	if (status & SR_INT_HRT) {
+
+		/* do latency calculations */
+		hrt_latency_update();
+
 		/* run any callouts that have met their deadline */
 		hrt_call_invoke();
 
@@ -799,8 +818,26 @@ hrt_call_reschedule()
 	}
 	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
 
-	/* set the new compare value */
-	rCCR_HRT = deadline & 0xffff;
+	/* set the new compare value and remember it for latency tracking */
+	rCCR_HRT = latency_baseline = deadline & 0xffff;
 }
 
+static void
+hrt_latency_update(void)
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
+
 #endif /* CONFIG_HRT_TIMER */