From 8cdbc4c5938069ccfa96c93fb6fb702a6b2fe2c4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 9 Feb 2020 14:17:24 -0500 Subject: [PATCH] px4iofirmware: add PX4IO_PERF define to completely disable perf counters --- platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c | 6 ++++- platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c | 6 ++++- .../nuttx/src/px4/stm/stm32_common/hrt/hrt.c | 6 ++++- platforms/posix/src/px4/common/drv_hrt.cpp | 6 ++++- platforms/qurt/src/px4/common/drv_hrt.cpp | 6 ++++- src/drivers/drv_hrt.h | 6 +++++ src/lib/perf/perf_counter.cpp | 6 ----- src/lib/perf/perf_counter.h | 6 ----- src/modules/px4iofirmware/CMakeLists.txt | 10 +++++-- src/modules/px4iofirmware/adc.c | 16 ++++++++++-- src/modules/px4iofirmware/controls.c | 25 +++++++++++++++++- src/modules/px4iofirmware/px4io.c | 17 +++++++++++- src/modules/px4iofirmware/px4io.h | 6 ----- src/modules/px4iofirmware/serial.c | 26 ++++++++++++++++++- 14 files changed, 118 insertions(+), 30 deletions(-) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c index 5587b49e6c..34d22ff3a0 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c @@ -48,7 +48,6 @@ #include #include -#include #include #include @@ -181,6 +180,11 @@ static uint32_t latency_baseline; /* timer count at interrupt (for latency purposes) */ static uint32_t latency_actual; +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT 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, void *args); diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c index 7e2954999b..1558d9b8d3 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c @@ -48,7 +48,6 @@ #include #include -#include #include #include @@ -179,6 +178,11 @@ static uint16_t latency_baseline; /* timer count at interrupt (for latency purposes) */ static uint16_t latency_actual; +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT 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, void *args); diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index 7ed244447e..5c78b6002f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -61,7 +61,6 @@ #include #include -#include #include "stm32_gpio.h" @@ -256,6 +255,11 @@ static uint16_t latency_baseline; /* timer count at interrupt (for latency purposes) */ static uint16_t latency_actual; +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT 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, void *arg); diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index f4c83ef11f..d41844668d 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include @@ -70,6 +69,11 @@ static uint64_t latency_baseline; /* timer count at interrupt (for latency purposes) */ static uint64_t latency_actual; +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + static px4_sem_t _hrt_lock; static struct work_s _hrt_work; diff --git a/platforms/qurt/src/px4/common/drv_hrt.cpp b/platforms/qurt/src/px4/common/drv_hrt.cpp index a1c078de41..c4cee64797 100644 --- a/platforms/qurt/src/px4/common/drv_hrt.cpp +++ b/platforms/qurt/src/px4/common/drv_hrt.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include @@ -66,6 +65,11 @@ static uint64_t latency_baseline; /* timer count at interrupt (for latency purposes) */ static uint64_t latency_actual; +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + static px4_sem_t _hrt_lock; static struct work_s _hrt_work; diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index fcbd19ccf4..2b625a4922 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -78,6 +78,12 @@ typedef struct hrt_call { void *arg; } *hrt_call_t; + +#define LATENCY_BUCKET_COUNT 8 +extern const uint16_t latency_bucket_count; +extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT]; +extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /** * Get absolute time in [us] (does not wrap). */ diff --git a/src/lib/perf/perf_counter.cpp b/src/lib/perf/perf_counter.cpp index 6c7c2456f4..6115de5d8d 100644 --- a/src/lib/perf/perf_counter.cpp +++ b/src/lib/perf/perf_counter.cpp @@ -48,12 +48,6 @@ #include "perf_counter.h" -/* latency histogram */ -const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; -const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; - - #ifdef __PX4_QURT // There is presumably no dprintf on QURT. Therefore use the usual output to mini-dm. #define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd)) diff --git a/src/lib/perf/perf_counter.h b/src/lib/perf/perf_counter.h index 64167166a2..d65a0908dd 100644 --- a/src/lib/perf/perf_counter.h +++ b/src/lib/perf/perf_counter.h @@ -42,12 +42,6 @@ #include #include -#define LATENCY_BUCKET_COUNT 8 - -extern const uint16_t latency_bucket_count; -extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT]; -extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; - /** * Counter types. */ diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 31dea33b5a..3b76556342 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +option(PX4IO_PERF "Enable px4io perf counters" OFF) + add_library(px4iofirmware adc.c controls.c @@ -39,7 +41,7 @@ add_library(px4iofirmware registers.c safety.c serial.c - ) +) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES px4iofirmware) target_link_libraries(px4iofirmware @@ -50,6 +52,10 @@ target_link_libraries(px4iofirmware nuttx_c mixer rc - perf output_limit ) + +if(PX4IO_PERF) + target_compile_definitions(px4iofirmware PRIVATE PX4IO_PERF) + target_link_libraries(px4iofirmware PRIVATE perf) +endif() diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 668b26cf53..1ac0fa2766 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -44,7 +44,10 @@ #include #include -#include + +#if defined(PX4IO_PERF) +# include +#endif #define DEBUG #include "px4io.h" @@ -76,12 +79,16 @@ #define rJDR4 REG(STM32_ADC_JDR4_OFFSET) #define rDR REG(STM32_ADC_DR_OFFSET) +#if defined(PX4IO_PERF) perf_counter_t adc_perf; +#endif int adc_init(void) { +#if defined(PX4IO_PERF) adc_perf = perf_alloc(PC_ELAPSED, "adc"); +#endif /* put the ADC into power-down mode */ rCR2 &= ~ADC_CR2_ADON; @@ -136,8 +143,9 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { - +#if defined(PX4IO_PERF) perf_begin(adc_perf); +#endif /* clear any previous EOC */ rSR = 0; @@ -154,7 +162,9 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { +#if defined(PX4IO_PERF) perf_end(adc_perf); +#endif return 0xffff; } } @@ -163,6 +173,8 @@ adc_measure(unsigned channel) uint16_t result = rDR; rSR = 0; +#if defined(PX4IO_PERF) perf_end(adc_perf); +#endif return result; } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index fc50afe03a..7161a86469 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -44,13 +44,16 @@ #include #include -#include #include #include #include #include #include +#if defined(PX4IO_PERF) +# include +#endif + #include "px4io.h" #define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ @@ -59,9 +62,11 @@ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated); +#if defined(PX4IO_PERF) static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; +#endif static int _dsm_fd = -1; int _sbus_fd = -1; @@ -78,7 +83,9 @@ static uint16_t _rssi = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { +#if defined(PX4IO_PERF) perf_begin(c_gather_dsm); +#endif uint8_t n_bytes = 0; uint8_t *bytes; bool dsm_11_bit; @@ -107,7 +114,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool } } +#if defined(PX4IO_PERF) perf_end(c_gather_dsm); +#endif /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, lost_count; @@ -196,9 +205,11 @@ controls_init(void) r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } +#if defined(PX4IO_PERF) c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +#endif } void @@ -239,7 +250,9 @@ controls_tick() _rssi = 0; } +#if defined(PX4IO_PERF) perf_begin(c_gather_sbus); +#endif bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, @@ -272,14 +285,18 @@ controls_tick() } +#if defined(PX4IO_PERF) perf_end(c_gather_sbus); +#endif /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have S.bus signal. */ +#if defined(PX4IO_PERF) perf_begin(c_gather_ppm); +#endif bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { @@ -289,12 +306,16 @@ controls_tick() r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } +#if defined(PX4IO_PERF) perf_end(c_gather_ppm); +#endif bool dsm_updated = false, st24_updated = false, sumd_updated = false; if (!((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) || (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM))) { +#if defined(PX4IO_PERF) perf_begin(c_gather_dsm); +#endif (void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -310,7 +331,9 @@ controls_tick() atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); } +#if defined(PX4IO_PERF) perf_end(c_gather_dsm); +#endif } /* limit number of channels to allowable data size */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 6ff641a0e9..1400051ca2 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -54,7 +54,10 @@ #include #include -#include +#if defined(PX4IO_PERF) +# include +#endif + #include #include @@ -345,6 +348,7 @@ user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); +#if defined(PX4IO_PERF) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -353,6 +357,7 @@ user_start(int argc, char *argv[]) /* and one for measuring the loop rate */ perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); +#endif struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; @@ -418,18 +423,28 @@ user_start(int argc, char *argv[]) dt = 0.02f; } +#if defined(PX4IO_PERF) /* track the rate at which the loop is running */ perf_count(loop_perf); /* kick the mixer */ perf_begin(mixer_perf); +#endif + mixer_tick(); + +#if defined(PX4IO_PERF) perf_end(mixer_perf); /* kick the control inputs */ perf_begin(controls_perf); +#endif + controls_tick(); + +#if defined(PX4IO_PERF) perf_end(controls_perf); +#endif /* some boards such as Pixhawk 2.1 made the unfortunate choice to combine the blue led channel with diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0a9d664ef0..0ca1819dbf 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -52,12 +52,6 @@ #include -/* - hotfix: we are critically short of memory in px4io and this is the - easiest way to reclaim about 800 bytes. - */ -#define perf_alloc(a,b) NULL - /* * Constants and limits. */ diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index d7dbf35118..cc106f017c 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -51,11 +51,13 @@ #include #include #include -#include //#define DEBUG #include "px4io.h" +#if defined(PX4IO_PERF) +# include + static perf_counter_t pc_txns; static perf_counter_t pc_errors; static perf_counter_t pc_ore; @@ -65,6 +67,7 @@ static perf_counter_t pc_idle; static perf_counter_t pc_badidle; static perf_counter_t pc_regerr; static perf_counter_t pc_crcerr; +#endif static void rx_handle_packet(void); static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); @@ -89,6 +92,7 @@ static struct IOPacket dma_packet; void interface_init(void) { +#if defined(PX4IO_PERF) pc_txns = perf_alloc(PC_ELAPSED, "txns"); pc_errors = perf_alloc(PC_COUNT, "errors"); pc_ore = perf_alloc(PC_COUNT, "overrun"); @@ -98,6 +102,7 @@ interface_init(void) pc_badidle = perf_alloc(PC_COUNT, "badidle"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); +#endif /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); @@ -160,7 +165,9 @@ rx_handle_packet(void) dma_packet.crc = 0; if (crc != crc_packet(&dma_packet)) { +#if defined(PX4IO_PERF) perf_count(pc_crcerr); +#endif /* send a CRC error reply */ dma_packet.count_code = PKT_CODE_CORRUPT; @@ -174,7 +181,10 @@ rx_handle_packet(void) /* it's a blind write - pass it on */ if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { +#if defined(PX4IO_PERF) perf_count(pc_regerr); +#endif + dma_packet.count_code = PKT_CODE_ERROR; } else { @@ -191,7 +201,10 @@ rx_handle_packet(void) uint16_t *registers; if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { +#if defined(PX4IO_PERF) perf_count(pc_regerr); +#endif + dma_packet.count_code = PKT_CODE_ERROR; } else { @@ -225,7 +238,9 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) * We are here because DMA completed, or UART reception stopped and * we think we have a packet in the buffer. */ +#if defined(PX4IO_PERF) perf_begin(pc_txns); +#endif /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -251,7 +266,9 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; +#if defined(PX4IO_PERF) perf_end(pc_txns); +#endif } static int @@ -266,6 +283,7 @@ serial_interrupt(int irq, void *context, FAR void *arg) USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ +#if defined(PX4IO_PERF) perf_count(pc_errors); if (sr & USART_SR_ORE) { @@ -280,6 +298,8 @@ serial_interrupt(int irq, void *context, FAR void *arg) perf_count(pc_fe); } +#endif + /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -311,7 +331,9 @@ serial_interrupt(int irq, void *context, FAR void *arg) if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */ +#if defined(PX4IO_PERF) perf_count(pc_badidle); +#endif dma_reset(); return 0; } @@ -320,7 +342,9 @@ serial_interrupt(int irq, void *context, FAR void *arg) * Looks like we received a packet. Stop the DMA and go process the * packet. */ +#if defined(PX4IO_PERF) perf_count(pc_idle); +#endif stm32_dmastop(rx_dma); rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); }