diff options
-rwxr-xr-x | Tools/px_uploader.py | 8 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_hrt.c | 41 |
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 */ |