diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 3fb20b81fb..6f17cca05a 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -291,6 +291,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set LED_RGB_MAXBRT 0"' // disable rgbled @@ -351,6 +352,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test @@ -420,6 +422,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test @@ -490,6 +493,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test @@ -561,6 +565,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test @@ -632,6 +637,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // critmon sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "critmon_start; sleep 5; critmon_stop"' // run tests @@ -709,6 +715,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // irqinfo sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "irqinfo"' // run tests @@ -771,6 +778,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests // sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // TODO: currently unusably slow // ensure buzzer is disabled for the next test @@ -847,6 +855,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-SEGGER_*`' // ensure buzzer is disabled for the next test @@ -916,6 +925,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 9a650bf459..d3ee20b980 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -148,8 +148,8 @@ if [[ $INSTALL_NUTTX == "true" ]]; then # add user to dialout group (serial port access) sudo usermod -a -G dialout $USER - - # Remove modem manager (interferes with PX4 serial port/USB serial usage). + + # Remove modem manager (interferes with PX4 serial port/USB serial usage). sudo apt-get remove modemmanager -y # arm-none-eabi-gcc diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 360acc4c42..655b39e95e 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -70,6 +70,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 70150a0d12..4c2276ef89 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -97,6 +97,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index cf02348489..62b7d6fd55 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -111,6 +111,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 7990f0f053..7fc6cce923 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -111,6 +111,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/auav/esc35-v1/default.cmake b/boards/auav/esc35-v1/default.cmake index 2c36fae924..606c066e48 100644 --- a/boards/auav/esc35-v1/default.cmake +++ b/boards/auav/esc35-v1/default.cmake @@ -55,5 +55,6 @@ px4_add_board( param top ver + work_queue ) diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 4547939cff..075920bee0 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -102,6 +102,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 056ae91921..8b0e606f51 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -102,6 +102,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 98b2ed4cd3..b37d97499c 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -67,6 +67,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 9a84cae6d2..a057859925 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -65,6 +65,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index b02720b3ee..c96239134e 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -66,5 +66,6 @@ px4_add_board( tune_control usb_connected ver + work_queue ) diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index ba51a3a381..d517442a43 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -74,6 +74,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 9080d8e58d..162abfc13f 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -72,6 +72,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 2548c0aa71..6c1aeb9beb 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -69,5 +69,6 @@ px4_add_board( topic_listener usb_connected ver + work_queue ) diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 5aca140c94..8a0a770ee6 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -81,6 +81,7 @@ px4_add_board( #topic_listener tune_control ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 81f618a6bd..37015ee33d 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -85,6 +85,7 @@ px4_add_board( #topic_listener tune_control ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 58dcb93aad..4313391427 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -106,6 +106,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 6624bba36b..f6ddf851f7 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -101,6 +101,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 0865106d1a..3e657ec810 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -93,6 +93,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 6446169931..bd4f9646c9 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -59,4 +59,6 @@ px4_add_board( topic_listener tune_control ver + work_queue + ) diff --git a/boards/px4/cannode-v1/default.cmake b/boards/px4/cannode-v1/default.cmake index 8075713e59..3fe33dd110 100644 --- a/boards/px4/cannode-v1/default.cmake +++ b/boards/px4/cannode-v1/default.cmake @@ -52,5 +52,6 @@ px4_add_board( reboot top ver + work_queue ) diff --git a/boards/px4/esc-v1/default.cmake b/boards/px4/esc-v1/default.cmake index 394441bed0..dcdb9a8c77 100644 --- a/boards/px4/esc-v1/default.cmake +++ b/boards/px4/esc-v1/default.cmake @@ -55,5 +55,6 @@ px4_add_board( param top ver + work_queue ) diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index e6eb8492ed..8e1712ea36 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -109,6 +109,7 @@ px4_add_board( tune_control #usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index c3aecf2e56..f2b4fb8c52 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -81,4 +81,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index f865f0dcfa..f2d2a8516a 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -104,6 +104,7 @@ px4_add_board( #topic_listener tune_control ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 929d27ce2b..f3c19283f5 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -80,4 +80,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 8c670995d4..4b1c3ffd53 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -72,4 +72,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 06938ae003..f187551193 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -104,6 +104,7 @@ px4_add_board( #topic_listener tune_control ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index eed8e5ed58..3e1b9ef3a7 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index b9084f1a00..107f0aebb5 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 51d7789a9b..cfe1efeb3e 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -109,6 +109,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 666d55467b..ba4dafbb2b 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -96,6 +96,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index fa8a3284d1..53711ca863 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -99,6 +99,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 31516f0e9f..e80640a310 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -96,6 +96,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index da368a0df7..1ca9081ee1 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -108,6 +108,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 18e179cd0f..b5d3bce982 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -108,6 +108,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index d7c796524a..8a598a750a 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 9f6f4f21b1..272a39576a 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index d4a134f48e..9dae74abe4 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -84,4 +84,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 8012a0c661..05ef47c317 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index cb6ff6fd8a..4175135720 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -92,4 +92,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 80e71d0dd2..340fc1b4c7 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -85,4 +85,6 @@ px4_add_board( tune_control usb_connected ver + work_queue + ) diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 0617cbf736..5dce25381b 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -108,6 +108,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 37c7766dae..8f4a733080 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -109,6 +109,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index d8d6288a2a..ca7251d855 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -110,6 +110,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index c603753936..43443ef45b 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -66,6 +66,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 944b2cfca4..0b9d1fcf3c 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -65,6 +65,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8ad9c8c8cc..98b058455e 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -69,6 +69,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index fe88dc2b9b..5a676e40aa 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -70,6 +70,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 1c18e6dbc3..737717daf6 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -69,6 +69,7 @@ px4_add_board( topic_listener tune_control ver + work_queue EXAMPLES bottle_drop # OBC challenge diff --git a/boards/thiemar/s2740vc-v1/default.cmake b/boards/thiemar/s2740vc-v1/default.cmake index 4c2fa67db9..7b134ba548 100644 --- a/boards/thiemar/s2740vc-v1/default.cmake +++ b/boards/thiemar/s2740vc-v1/default.cmake @@ -45,5 +45,6 @@ px4_add_board( reboot top ver + work_queue ) diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 75c2fd9936..56d5df30e8 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -103,6 +103,7 @@ px4_add_board( tune_control usb_connected ver + work_queue EXAMPLES #bottle_drop # OBC challenge diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp index 7bcd39f7c9..6789d4bea4 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp @@ -44,9 +44,6 @@ class ScheduledWorkItem : public WorkItem { public: - ScheduledWorkItem(const wq_config_t &config) : WorkItem(config) {} - virtual ~ScheduledWorkItem() override; - /** * Schedule next run with a delay in microseconds. * @@ -58,7 +55,7 @@ public: * Schedule repeating run with optional delay. * * @param interval_us The interval in microseconds. - * @param delay_us The delay (optional) in microseconds. + * @param delay_us The delay (optional) in microseconds. */ void ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us = 0); @@ -67,14 +64,20 @@ public: */ void ScheduleClear(); - virtual void Run() override = 0; +protected: + + ScheduledWorkItem(const char *name, const wq_config_t &config) : WorkItem(name, config) {} + virtual ~ScheduledWorkItem() override; + + virtual void print_run_status() const override; private: + virtual void Run() override = 0; + static void schedule_trampoline(void *arg); hrt_call _call{}; - }; } // namespace px4 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 3329825499..a99099e295 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -33,7 +33,6 @@ #pragma once - #include "WorkQueueManager.hpp" #include "WorkQueue.hpp" @@ -41,21 +40,18 @@ #include #include +#include + namespace px4 { -class WorkItem : public IntrusiveQueueNode +class WorkItem : public ListNode, public IntrusiveQueueNode { public: - explicit WorkItem(const wq_config_t &config); - WorkItem() = delete; - - virtual ~WorkItem(); - inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); } - virtual void Run() = 0; + virtual void print_run_status() const; /** * Switch to a different WorkQueue. @@ -68,6 +64,18 @@ public: protected: + explicit WorkItem(const char *name, const wq_config_t &config); + WorkItem() = delete; + + virtual ~WorkItem(); + +protected: + + void RunPreamble() { _run_count++; } + + friend void WorkQueue::Run(); + virtual void Run() = 0; + /** * Initialize WorkItem given a WorkQueue config. This call * can also be used to switch to a different WorkQueue. @@ -79,9 +87,18 @@ protected: bool Init(const wq_config_t &config); void Deinit(); + float elapsed_time() const; + float average_rate() const; + float average_interval() const; + + + hrt_abstime _start_time{0}; + unsigned _run_count{0}; + const char *_item_name; + private: - WorkQueue *_wq{nullptr}; + WorkQueue *_wq{nullptr}; }; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index ab4ad1f1b7..6019c890ee 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -35,6 +35,7 @@ #include "WorkQueueManager.hpp" +#include #include #include #include @@ -57,6 +58,9 @@ public: const char *get_name() { return _config.name; } + bool Attach(WorkItem *item); + void Detach(WorkItem *item); + void Add(WorkItem *item); void Remove(WorkItem *item); @@ -66,7 +70,7 @@ public: void request_stop() { _should_exit.store(true); } - void print_status(); + void print_status(bool last = false); private: @@ -84,10 +88,10 @@ private: #endif IntrusiveQueue _q; - px4_sem_t _process_lock; - - px4::atomic_bool _should_exit{false}; - const wq_config_t &_config; + px4_sem_t _process_lock; + const wq_config_t &_config; + BlockingList _work_items; + px4::atomic_bool _should_exit{false}; }; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 2f0cec8b06..f2de048913 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -82,6 +82,11 @@ int WorkQueueManagerStart(); */ int WorkQueueManagerStop(); +/** + * Work queue manager status. + */ +int WorkQueueManagerStatus(); + /** * Create (or find) a work queue with a particular configuration. * diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index 72c45598a7..9e96d6c77b 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -41,25 +41,44 @@ ScheduledWorkItem::~ScheduledWorkItem() ScheduleClear(); } -void ScheduledWorkItem::schedule_trampoline(void *arg) +void +ScheduledWorkItem::schedule_trampoline(void *arg) { ScheduledWorkItem *dev = reinterpret_cast(arg); dev->ScheduleNow(); } -void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us) +void +ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us) { hrt_call_after(&_call, delay_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } -void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us) +void +ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us) { + // reset start time to first deadline (approximately) + _start_time = hrt_absolute_time() + interval_us + delay_us; + hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } -void ScheduledWorkItem::ScheduleClear() +void +ScheduledWorkItem::ScheduleClear() { hrt_cancel(&_call); } +void +ScheduledWorkItem::print_run_status() const +{ + if (_call.period > 0) { + PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), + (double)average_interval(), _call.period); + + } else { + WorkItem::print_run_status(); + } +} + } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index 865df26e50..a2e3b2332e 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -42,7 +42,8 @@ namespace px4 { -WorkItem::WorkItem(const wq_config_t &config) +WorkItem::WorkItem(const char *name, const wq_config_t &config) : + _item_name(name) { if (!Init(config)) { PX4_ERR("init failed"); @@ -54,25 +55,26 @@ WorkItem::~WorkItem() Deinit(); } -bool WorkItem::Init(const wq_config_t &config) +bool +WorkItem::Init(const wq_config_t &config) { // clear any existing first Deinit(); px4::WorkQueue *wq = WorkQueueFindOrCreate(config); - if (wq == nullptr) { - PX4_ERR("%s not available", config.name); - - } else { + if ((wq != nullptr) && wq->Attach(this)) { _wq = wq; + _start_time = hrt_absolute_time(); return true; } + PX4_ERR("%s not available", config.name); return false; } -void WorkItem::Deinit() +void +WorkItem::Deinit() { // remove any currently queued work if (_wq != nullptr) { @@ -80,8 +82,47 @@ void WorkItem::Deinit() px4::WorkQueue *wq_temp = _wq; _wq = nullptr; + // remove any queued work wq_temp->Remove(this); + + wq_temp->Detach(this); + } +} + +float +WorkItem::elapsed_time() const +{ + return hrt_elapsed_time(&_start_time) / 1e6f; +} + +float +WorkItem::average_rate() const +{ + const float rate = _run_count / elapsed_time(); + + if (PX4_ISFINITE(rate)) { + return rate; } + + return 0.0f; +} + +float +WorkItem::average_interval() const +{ + const float interval = 1000000.0f / average_rate(); + + if (PX4_ISFINITE(interval)) { + return interval; + } + + return 0.0f; +} + +void +WorkItem::print_run_status() const +{ + PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us\n", _item_name, (double)average_rate(), (double)average_interval()); } } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index 41641bae18..22a8d79505 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -72,10 +72,44 @@ WorkQueue::~WorkQueue() #endif /* __PX4_NUTTX */ } -void WorkQueue::Add(WorkItem *item) +bool +WorkQueue::Attach(WorkItem *item) { - // TODO: prevent additions when shutting down + work_lock(); + + if (!should_exit()) { + _work_items.add(item); + work_unlock(); + return true; + } + work_unlock(); + return false; +} + +void +WorkQueue::Detach(WorkItem *item) +{ + work_lock(); + + _work_items.remove(item); + + if (_work_items.size() == 0) { + // shutdown, no active WorkItems + PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name); + + request_stop(); + + // Wake up the worker thread + px4_sem_post(&_process_lock); + } + + work_unlock(); +} + +void +WorkQueue::Add(WorkItem *item) +{ work_lock(); _q.push(item); work_unlock(); @@ -84,14 +118,16 @@ void WorkQueue::Add(WorkItem *item) px4_sem_post(&_process_lock); } -void WorkQueue::Remove(WorkItem *item) +void +WorkQueue::Remove(WorkItem *item) { work_lock(); _q.remove(item); work_unlock(); } -void WorkQueue::Clear() +void +WorkQueue::Clear() { work_lock(); @@ -102,7 +138,8 @@ void WorkQueue::Clear() work_unlock(); } -void WorkQueue::Run() +void +WorkQueue::Run() { while (!should_exit()) { px4_sem_wait(&_process_lock); @@ -114,17 +151,43 @@ void WorkQueue::Run() WorkItem *work = _q.pop(); work_unlock(); // unlock work queue to run (item may requeue itself) + work->RunPreamble(); work->Run(); work_lock(); // re-lock } work_unlock(); } + + PX4_DEBUG("%s: exiting", _config.name); } -void WorkQueue::print_status() +void +WorkQueue::print_status(bool last) { - PX4_INFO("WorkQueue: %s running", get_name()); + const size_t num_items = _work_items.size(); + PX4_INFO_RAW("%-16s\n", get_name()); + size_t i = 0; + + for (WorkItem *item : _work_items) { + i++; + + if (last) { + PX4_INFO_RAW(" "); + + } else { + PX4_INFO_RAW("| "); + } + + if (i < num_items) { + PX4_INFO_RAW("|__ %zu) ", i); + + } else { + PX4_INFO_RAW("\\__ %zu) ", i); + } + + item->print_run_status(); + } } } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 471299a55d..1e7c4479d3 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -59,10 +59,11 @@ static BlockingList *_wq_manager_wqs_list{nullptr}; // queue of WorkQueues to be created (as threads in the wq manager task) static BlockingQueue *_wq_manager_create_queue{nullptr}; -static px4::atomic_bool _wq_manager_should_exit{false}; +static px4::atomic_bool _wq_manager_should_exit{true}; -static WorkQueue *FindWorkQueueByName(const char *name) +static WorkQueue * +FindWorkQueueByName(const char *name) { if (_wq_manager_wqs_list == nullptr) { PX4_ERR("not running"); @@ -81,7 +82,8 @@ static WorkQueue *FindWorkQueueByName(const char *name) return nullptr; } -WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq) +WorkQueue * +WorkQueueFindOrCreate(const wq_config_t &new_wq) { if (_wq_manager_create_queue == nullptr) { PX4_ERR("not running"); @@ -116,7 +118,8 @@ WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq) return wq; } -const wq_config_t &device_bus_to_wq(uint32_t device_id_int) +const wq_config_t & +device_bus_to_wq(uint32_t device_id_int) { union device::Device::DeviceId device_id; device_id.devid = device_id_int; @@ -155,7 +158,8 @@ const wq_config_t &device_bus_to_wq(uint32_t device_id_int) return wq_configurations::hp_default; }; -static void *WorkQueueRunner(void *context) +static void * +WorkQueueRunner(void *context) { wq_config_t *config = static_cast(context); WorkQueue wq(*config); @@ -171,7 +175,8 @@ static void *WorkQueueRunner(void *context) return nullptr; } -static int WorkQueueManagerRun(int, char **) +static int +WorkQueueManagerRun(int, char **) { _wq_manager_wqs_list = new BlockingList(); _wq_manager_create_queue = new BlockingQueue(); @@ -232,7 +237,7 @@ static int WorkQueueManagerRun(int, char **) int ret_create = pthread_create(&thread, &attr, WorkQueueRunner, (void *)wq); if (ret_create == 0) { - PX4_INFO("creating: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize); + PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize); } else { PX4_ERR("failed to create thread for %s (%i): %s", wq->name, ret_create, strerror(ret_create)); @@ -250,46 +255,113 @@ static int WorkQueueManagerRun(int, char **) return 0; } -int WorkQueueManagerStart() +int +WorkQueueManagerStart() { - int task_id = px4_task_spawn_cmd("wq:manager", - SCHED_DEFAULT, - PX4_WQ_HP_BASE, - 1200, - (px4_main_t)&WorkQueueManagerRun, - nullptr); - - if (task_id < 0) { - PX4_ERR("task start failed (%i)", task_id); - return -errno; + if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) { + + _wq_manager_should_exit.store(false); + + int task_id = px4_task_spawn_cmd("wq:manager", + SCHED_DEFAULT, + PX4_WQ_HP_BASE, + 1280, + (px4_main_t)&WorkQueueManagerRun, + nullptr); + + if (task_id < 0) { + _wq_manager_should_exit.store(true); + PX4_ERR("task start failed (%i)", task_id); + return -errno; + } + + } else { + PX4_WARN("already running"); + return PX4_ERROR; } - return 0; + return PX4_OK; } -int WorkQueueManagerStop() +int +WorkQueueManagerStop() { - if (_wq_manager_wqs_list != nullptr) { - auto lg = _wq_manager_wqs_list->getLockGuard(); + if (!_wq_manager_should_exit.load()) { - // ask all work queues (threads) to stop - // NOTE: not currently safe without all WorkItems stopping first - for (WorkQueue *wq : *_wq_manager_wqs_list) { - wq->request_stop(); + // error can't shutdown until all WorkItems are removed/stopped + if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) { + PX4_ERR("can't shutdown with active WQs"); + WorkQueueManagerStatus(); + return PX4_ERROR; } - delete _wq_manager_wqs_list; + // first ask all WQs to stop + if (_wq_manager_wqs_list != nullptr) { + { + auto lg = _wq_manager_wqs_list->getLockGuard(); + + // ask all work queues (threads) to stop + // NOTE: not currently safe without all WorkItems stopping first + for (WorkQueue *wq : *_wq_manager_wqs_list) { + wq->request_stop(); + } + } + + // wait until they're all stopped (empty list) + while (_wq_manager_wqs_list->size() > 0) { + px4_usleep(1000); + } + + delete _wq_manager_wqs_list; + } + + _wq_manager_should_exit.store(true); + + if (_wq_manager_create_queue != nullptr) { + // push nullptr to wake the wq manager task + _wq_manager_create_queue->push(nullptr); + + px4_usleep(10000); + + delete _wq_manager_create_queue; + } + + } else { + PX4_WARN("not running"); + return PX4_ERROR; } - _wq_manager_should_exit.store(true); + return PX4_OK; +} - if (_wq_manager_create_queue != nullptr) { - // push nullptr to wake the wq manager task - _wq_manager_create_queue->push(nullptr); +int +WorkQueueManagerStatus() +{ + if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) { + + const size_t num_wqs = _wq_manager_wqs_list->size(); + PX4_INFO_RAW("\nWork Queue: %-1zu threads RATE INTERVAL\n", num_wqs); + + auto lg = _wq_manager_wqs_list->getLockGuard(); + size_t i = 0; - px4_usleep(1000); + for (WorkQueue *wq : *_wq_manager_wqs_list) { + i++; + + const bool last_wq = !(i < num_wqs); + + if (!last_wq) { + PX4_INFO_RAW("|__ %zu) ", i); + + } else { + PX4_INFO_RAW("\\__ %zu) ", i); + } + + wq->print_status(last_wq); + } - delete _wq_manager_create_queue; + } else { + PX4_INFO("not running"); } return PX4_OK; diff --git a/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h b/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h index 52ba1c98b2..dac4c227c4 100644 --- a/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h +++ b/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h @@ -42,7 +42,7 @@ using namespace px4; class WQueueScheduledTest : public px4::ScheduledWorkItem { public: - WQueueScheduledTest() : px4::ScheduledWorkItem(px4::wq_configurations::test2) {} + WQueueScheduledTest() : px4::ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test2) {} ~WQueueScheduledTest() = default; int main(); diff --git a/platforms/common/px4_work_queue/test/wqueue_test.h b/platforms/common/px4_work_queue/test/wqueue_test.h index 573fea9007..dddf08396a 100644 --- a/platforms/common/px4_work_queue/test/wqueue_test.h +++ b/platforms/common/px4_work_queue/test/wqueue_test.h @@ -42,7 +42,7 @@ using namespace px4; class WQueueTest : public px4::WorkItem { public: - WQueueTest() : px4::WorkItem(px4::wq_configurations::test1) {} + WQueueTest() : px4::WorkItem("WQueueTest", px4::wq_configurations::test1) {} ~WQueueTest() = default; int main(); diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 60b3408508..a1bd4dcdfa 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -75,6 +75,8 @@ perf sleep 1 perf latency sleep 1 +work_queue status +sleep 1 uorb top -1 sleep 1 diff --git a/src/drivers/adc/adc.cpp b/src/drivers/adc/adc.cpp index 1bfe17c83e..97a7f0b1de 100644 --- a/src/drivers/adc/adc.cpp +++ b/src/drivers/adc/adc.cpp @@ -104,7 +104,7 @@ private: ADC::ADC(uint32_t base_address, uint32_t channels) : CDev(ADC0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _base_address(base_address) { diff --git a/src/drivers/barometer/bmp280/bmp280.cpp b/src/drivers/barometer/bmp280/bmp280.cpp index 6e765f66bc..5b7cd12df4 100644 --- a/src/drivers/barometer/bmp280/bmp280.cpp +++ b/src/drivers/barometer/bmp280/bmp280.cpp @@ -112,7 +112,7 @@ extern "C" __EXPORT int bmp280_main(int argc, char *argv[]); BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _running(false), _report_interval(0), diff --git a/src/drivers/barometer/lps22hb/LPS22HB.cpp b/src/drivers/barometer/lps22hb/LPS22HB.cpp index 0df0efd1aa..ac980f058e 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.cpp @@ -44,7 +44,7 @@ LPS22HB::LPS22HB(device::Device *interface, const char *path) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, "lps22hb_read")), _comms_errors(perf_alloc(PC_COUNT, "lps22hb_comms_errors")) diff --git a/src/drivers/barometer/lps25h/lps25h.cpp b/src/drivers/barometer/lps25h/lps25h.cpp index f9f95fd034..55b57d2bf8 100644 --- a/src/drivers/barometer/lps25h/lps25h.cpp +++ b/src/drivers/barometer/lps25h/lps25h.cpp @@ -261,7 +261,7 @@ extern "C" __EXPORT int lps25h_main(int argc, char *argv[]); LPS25H::LPS25H(device::Device *interface, const char *path) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")), _comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors")) diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp index 5a58b06f09..6bad1cac6c 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2.cpp @@ -148,7 +148,7 @@ extern "C" __EXPORT int mpl3115a2_main(int argc, char *argv[]); MPL3115A2::MPL3115A2(device::Device *interface, const char *path) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _measure_interval(0), _reports(nullptr), diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 00d18f395a..30f7646571 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -183,7 +183,7 @@ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _prom(prom_buf.s), _reports(nullptr), diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index be04ba14db..f1edd0c14d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -46,7 +46,7 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) : - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _cycle(perf_alloc(PC_ELAPSED, "batt_smbus_cycle")), _batt_topic(nullptr), diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index f731110bff..ba65f7c909 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -49,7 +49,7 @@ CameraCapture *g_camera_capture{nullptr}; } CameraCapture::CameraCapture() : - ScheduledWorkItem(px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { memset(&_work_publisher, 0, sizeof(_work_publisher)); diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 7c2d93dd86..d1d892b19a 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -233,7 +233,7 @@ CameraTrigger *g_camera_trigger; } CameraTrigger::CameraTrigger() : - ScheduledWorkItem(px4::wq_configurations::lp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), _engagecall {}, _disengagecall {}, _engage_turn_on_off_call {}, diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 0a365b5b13..7ec9da90c4 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -84,7 +84,7 @@ static constexpr unsigned char crc_lsb_vector[] = { }; CM8JL65::CM8JL65(const char *port, uint8_t rotation) : - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation) { // Store the port name. diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp index 50ef8a8c25..82e0fc401a 100644 --- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -174,7 +174,7 @@ private: LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation): CDev(device_path), - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation) { _serial_port = strdup(serial_port); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp index ae4f33d19d..19eb40da0f 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp @@ -52,7 +52,6 @@ LidarLite::LidarLite(const uint8_t rotation) : LidarLite::~LidarLite() { perf_free(_sample_perf); - perf_free(_sample_interval_perf); perf_free(_comms_errors); perf_free(_sensor_resets); perf_free(_sensor_zero_resets); @@ -62,7 +61,6 @@ void LidarLite::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_sample_interval_perf); perf_print_counter(_comms_errors); perf_print_counter(_sensor_resets); perf_print_counter(_sensor_zero_resets); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h index e86709e01f..266c762ef8 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h @@ -90,7 +90,6 @@ protected: PX4Rangefinder _px4_rangefinder; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "ll40ls: comms errors")}; - perf_counter_t _sample_interval_perf{perf_alloc(PC_ELAPSED, "ll40ls: interval")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")}; perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")}; perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")}; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index f083d2f972..c1963cb932 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -44,7 +44,7 @@ LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) : LidarLite(rotation), I2C("LL40LS", nullptr, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { // up the retries since the device misses the first measure attempts _retries = 3; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp index eb2005089a..6184a02f21 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp @@ -46,7 +46,7 @@ LidarLitePWM::LidarLitePWM(const uint8_t rotation) : LidarLite(rotation), - ScheduledWorkItem(px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { } diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 3500908ab0..d1f2df8377 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -225,7 +225,7 @@ private: MappyDot::MappyDot(const int bus) : I2C("MappyDot", MAPPYDOT_DEVICE_PATH, bus, MAPPYDOT_BASE_ADDR, MAPPYDOT_BUS_CLOCK), ModuleParams(nullptr), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) {} MappyDot::~MappyDot() diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index 3541062eae..7051745558 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -181,7 +181,7 @@ private: MB12XX::MB12XX(const int bus) : I2C("MB12xx", MB12XX_DEVICE_PATH, bus, MB12XX_BASE_ADDR, MB12XX_BUS_SPEED), ModuleParams(nullptr), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { } diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index 817461560b..ad99ecff02 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -157,7 +157,7 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); SF0X::SF0X(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _rotation(rotation), _min_distance(0.30f), _max_distance(40.0f), diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 179a7bb3da..300545f94b 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -167,7 +167,7 @@ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]); SF1XX::SF1XX(uint8_t rotation, int bus, int address) : I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _rotation(rotation) { } diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index ac0c679825..0b1c791f75 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -172,7 +172,7 @@ extern "C" __EXPORT int srf02_main(int argc, char *argv[]); SRF02::SRF02(uint8_t rotation, int bus, int address) : I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _rotation(rotation) { } diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 56a2f2a1b3..c18491b219 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -192,7 +192,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) TERARANGER::TERARANGER(const int bus, const uint8_t orientation, const int address) : I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _orientation(orientation) { // up the retries since the device misses the first measure attempts diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 8418544f14..a361a11a8f 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -34,7 +34,7 @@ #include "TFMINI.hpp" TFMINI::TFMINI(const char *port, uint8_t rotation) : - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) { // store port name diff --git a/src/drivers/distance_sensor/ulanding/ulanding.cpp b/src/drivers/distance_sensor/ulanding/ulanding.cpp index 5ba7549613..ce684dde16 100644 --- a/src/drivers/distance_sensor/ulanding/ulanding.cpp +++ b/src/drivers/distance_sensor/ulanding/ulanding.cpp @@ -172,7 +172,7 @@ private: Radar::Radar(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _rotation(rotation) { /* store port name */ diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index f641315833..1194859562 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -167,7 +167,7 @@ private: VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _rotation(rotation) { // Allow 3 retries as the device typically misses the first measure attempts. diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 48cb7a2cd1..4e442d02ca 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -51,7 +51,7 @@ Heater::Heater() : ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { px4_arch_configgpio(GPIO_HEATER_OUTPUT); px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 0b9180f847..a6ca681d68 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -39,7 +39,7 @@ ADIS16448::ADIS16448(int bus, uint32_t device, enum Rotation rotation) : SPI("ADIS16448", nullptr, bus, device, SPIDEV_MODE3, 1000000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _px4_baro(get_device_id(), ORB_PRIO_MAX), _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), @@ -64,7 +64,6 @@ ADIS16448::~ADIS16448() // Delete the perf counter. perf_free(_perf_read); - perf_free(_perf_read_interval); perf_free(_perf_transfer); perf_free(_perf_bad_transfer); perf_free(_perf_crc_bad); @@ -314,7 +313,6 @@ void ADIS16448::print_info() { perf_print_counter(_perf_read); - perf_print_counter(_perf_read_interval); perf_print_counter(_perf_transfer); perf_print_counter(_perf_bad_transfer); perf_print_counter(_perf_crc_bad); @@ -444,8 +442,6 @@ convert12BitToINT16(uint16_t word) int ADIS16448::measure() { - perf_count(_perf_read_interval); - // Start measuring. perf_begin(_perf_read); diff --git a/src/drivers/imu/adis16448/ADIS16448.h b/src/drivers/imu/adis16448/ADIS16448.h index e5fd10d8e2..8e9378a828 100644 --- a/src/drivers/imu/adis16448/ADIS16448.h +++ b/src/drivers/imu/adis16448/ADIS16448.h @@ -211,7 +211,6 @@ private: static constexpr float _sample_rate{ADIS16448_ACCEL_GYRO_UPDATE_RATE}; perf_counter_t _perf_read{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: read"))}; - perf_counter_t _perf_read_interval{perf_alloc(PC_INTERVAL, "ADIS16448: read interval")}; perf_counter_t _perf_transfer{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: transfer"))}; perf_counter_t _perf_bad_transfer{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: bad transfers"))}; perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: CRC16 bad"))}; diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index b7f749e0b4..aa3c36a528 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -55,10 +55,9 @@ using namespace time_literals; ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) : SPI("ADIS16477", nullptr, bus, device, SPIDEV_MODE3, 1000000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), - _sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16477: read interval")), _sample_perf(perf_alloc(PC_ELAPSED, "adis16477: read")), _bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers")) { @@ -82,7 +81,6 @@ ADIS16477::~ADIS16477() stop(); // delete the perf counters - perf_free(_sample_interval_perf); perf_free(_sample_perf); perf_free(_bad_transfers); } @@ -321,7 +319,6 @@ ADIS16477::Run() int ADIS16477::measure() { - perf_count(_sample_interval_perf); perf_begin(_sample_perf); // Fetch the full set of measurements from the ADIS16477 in one pass (burst read). @@ -391,7 +388,6 @@ ADIS16477::measure() void ADIS16477::print_info() { - perf_print_counter(_sample_interval_perf); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index cc7cec9dd6..8203cfa7e9 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -65,7 +65,6 @@ private: PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; - perf_counter_t _sample_interval_perf; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 7225161b60..60c8200148 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -68,10 +68,9 @@ using namespace time_literals; ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) : SPI("ADIS16497", nullptr, bus, device, SPIDEV_MODE3, 5000000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), - _sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16497: read interval")), _sample_perf(perf_alloc(PC_ELAPSED, "adis16497: read")), _bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers")) { @@ -95,7 +94,6 @@ ADIS16497::~ADIS16497() stop(); // delete the perf counters - perf_free(_sample_interval_perf); perf_free(_sample_perf); perf_free(_bad_transfers); } @@ -384,7 +382,6 @@ ADIS16497::Run() int ADIS16497::measure() { - perf_count(_sample_interval_perf); perf_begin(_sample_perf); // Fetch the full set of measurements from the ADIS16497 in one pass (burst read). @@ -468,7 +465,6 @@ ADIS16497::measure() void ADIS16497::print_info() { - perf_print_counter(_sample_interval_perf); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp index 7cb0e7fdf5..8eb9d62b88 100644 --- a/src/drivers/imu/adis16497/ADIS16497.hpp +++ b/src/drivers/imu/adis16497/ADIS16497.hpp @@ -101,7 +101,6 @@ private: PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; - perf_counter_t _sample_interval_perf; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index ea63dd20d4..34970f4a7e 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -223,7 +223,7 @@ private: BMA180::BMA180(int bus, uint32_t device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), - ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), _call_interval(0), _reports(nullptr), _accel_range_scale(0.0f), diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index 2ac4a539e5..c367264808 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -46,10 +46,9 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")), - _measure_interval(perf_alloc(PC_INTERVAL, "bmi055_accel_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")), _duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")), @@ -65,7 +64,6 @@ BMI055_accel::~BMI055_accel() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_measure_interval); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_duplicates); @@ -286,8 +284,6 @@ BMI055_accel::check_registers(void) void BMI055_accel::measure() { - perf_count(_measure_interval); - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -409,7 +405,6 @@ void BMI055_accel::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_measure_interval); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); diff --git a/src/drivers/imu/bmi055/BMI055_accel.hpp b/src/drivers/imu/bmi055/BMI055_accel.hpp index 867151a278..f449d1069b 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.hpp +++ b/src/drivers/imu/bmi055/BMI055_accel.hpp @@ -169,7 +169,6 @@ private: PX4Accelerometer _px4_accel; perf_counter_t _sample_perf; - perf_counter_t _measure_interval; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _duplicates; diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index 8d135f890d..b1f81926df 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -49,10 +49,9 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")), - _measure_interval(perf_alloc(PC_INTERVAL, "bmi055_gyro_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")) { @@ -66,7 +65,6 @@ BMI055_gyro::~BMI055_gyro() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_measure_interval); perf_free(_bad_transfers); perf_free(_bad_registers); } @@ -322,8 +320,6 @@ BMI055_gyro::check_registers(void) void BMI055_gyro::measure() { - perf_count(_measure_interval); - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -421,7 +417,6 @@ BMI055_gyro::print_info() PX4_INFO("Gyro"); perf_print_counter(_sample_perf); - perf_print_counter(_measure_interval); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp index e5981b9332..a2d77178b4 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.hpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp @@ -161,7 +161,6 @@ private: PX4Gyroscope _px4_gyro; perf_counter_t _sample_perf; - perf_counter_t _measure_interval; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 14d4320870..1bf94243aa 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -54,10 +54,9 @@ const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTER BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")), - _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_accel_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")), _duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")), @@ -73,7 +72,6 @@ BMI088_accel::~BMI088_accel() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_measure_interval); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_duplicates); @@ -398,8 +396,6 @@ BMI088_accel::check_registers(void) void BMI088_accel::measure() { - perf_count(_measure_interval); - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -539,7 +535,6 @@ BMI088_accel::print_info() PX4_INFO("Accel"); perf_print_counter(_sample_perf); - perf_print_counter(_measure_interval); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); diff --git a/src/drivers/imu/bmi088/BMI088_accel.hpp b/src/drivers/imu/bmi088/BMI088_accel.hpp index 20e89ff009..b03543bd7e 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.hpp +++ b/src/drivers/imu/bmi088/BMI088_accel.hpp @@ -190,7 +190,6 @@ private: PX4Accelerometer _px4_accel; perf_counter_t _sample_perf; - perf_counter_t _measure_interval; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _duplicates; diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index 8068003694..270560d8c2 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -57,10 +57,9 @@ const uint8_t BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS] BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")), - _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_gyro_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")) { @@ -74,7 +73,6 @@ BMI088_gyro::~BMI088_gyro() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_measure_interval); perf_free(_bad_transfers); perf_free(_bad_registers); } @@ -340,8 +338,6 @@ BMI088_gyro::check_registers(void) void BMI088_gyro::measure() { - perf_count(_measure_interval); - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -435,7 +431,6 @@ BMI088_gyro::print_info() PX4_INFO("Gyro"); perf_print_counter(_sample_perf); - perf_print_counter(_measure_interval); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); diff --git a/src/drivers/imu/bmi088/BMI088_gyro.hpp b/src/drivers/imu/bmi088/BMI088_gyro.hpp index 7b7c73b622..4fc87f3850 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.hpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.hpp @@ -159,7 +159,6 @@ private: PX4Gyroscope _px4_gyro; perf_counter_t _sample_perf; - perf_counter_t _measure_interval; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 04a11c18f3..6095d7dc1b 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -51,7 +51,7 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) : SPI("BMI160", nullptr, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), _accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")), diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 08aa80aa42..5cc111b383 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -188,10 +188,9 @@ using namespace time_literals; FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) : SPI("FXAS21002C", nullptr, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _sample_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": read interval")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicate reading")) @@ -206,7 +205,6 @@ FXAS21002C::~FXAS21002C() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_sample_interval_perf); perf_free(_errors); perf_free(_bad_registers); perf_free(_duplicates); @@ -530,7 +528,6 @@ FXAS21002C::measure() { // start the performance counter perf_begin(_sample_perf); - perf_count(_sample_interval_perf); /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -598,7 +595,6 @@ FXAS21002C::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - perf_print_counter(_sample_interval_perf); perf_print_counter(_errors); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp index da7496596d..8b0f0b15eb 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp @@ -81,7 +81,6 @@ private: unsigned _read{0}; perf_counter_t _sample_perf; - perf_counter_t _sample_interval_perf; perf_counter_t _errors; perf_counter_t _bad_registers; perf_counter_t _duplicates; diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index c5ecc1c1c7..c4d54ac9dc 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -55,14 +55,13 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) : SPI("FXOS8701CQ", nullptr, bus, device, SPIDEV_MODE0, 1 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), ORB_PRIO_LOW, rotation), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")), #endif _accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")), - _accel_sample_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": acc read interval")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")), _accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe")) { @@ -87,7 +86,6 @@ FXOS8701CQ::~FXOS8701CQ() // delete the perf counter perf_free(_accel_sample_perf); - perf_free(_accel_sample_interval_perf); perf_free(_bad_registers); perf_free(_accel_duplicates); } @@ -334,7 +332,6 @@ FXOS8701CQ::Run() { // start the performance counter perf_begin(_accel_sample_perf); - perf_count(_accel_sample_interval_perf); // status register and data as read back from the device #pragma pack(push, 1) @@ -413,7 +410,6 @@ void FXOS8701CQ::print_info() { perf_print_counter(_accel_sample_perf); - perf_print_counter(_accel_sample_interval_perf); #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) perf_print_counter(_mag_sample_perf); diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp index 26bcbbe232..a5528d918f 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -214,7 +214,6 @@ private: unsigned _accel_samplerate{FXOS8701C_ACCEL_DEFAULT_RATE}; perf_counter_t _accel_sample_perf; - perf_counter_t _accel_sample_interval_perf; perf_counter_t _bad_registers; perf_counter_t _accel_duplicates; diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index e499fc7756..072c391501 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -86,7 +86,7 @@ const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGIST ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only) : - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), diff --git a/src/drivers/imu/l3gd20/l3gd20.cpp b/src/drivers/imu/l3gd20/l3gd20.cpp index 6cadddb996..5f727147e8 100644 --- a/src/drivers/imu/l3gd20/l3gd20.cpp +++ b/src/drivers/imu/l3gd20/l3gd20.cpp @@ -322,7 +322,7 @@ constexpr uint8_t L3GD20::_checked_registers[]; L3GD20::L3GD20(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _errors(perf_alloc(PC_COUNT, "l3gd20_err")), diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 3ecf4a021b..659242d280 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -55,13 +55,11 @@ static constexpr uint8_t _checked_registers[] = { LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) : SPI("LSM303D", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), - _accel_sample_interval_perf(perf_alloc(PC_INTERVAL, "lsm303d: acc interval")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")), - _mag_sample_interval_perf(perf_alloc(PC_INTERVAL, "lsm303d: mag interval")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d: bad_reg")), _bad_values(perf_alloc(PC_COUNT, "lsm303d: bad_val")), _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d: acc_dupe")) @@ -78,9 +76,7 @@ LSM303D::~LSM303D() // delete the perf counter perf_free(_accel_sample_perf); - perf_free(_accel_sample_interval_perf); perf_free(_mag_sample_perf); - perf_free(_mag_sample_interval_perf); perf_free(_bad_registers); perf_free(_bad_values); perf_free(_accel_duplicates); @@ -470,7 +466,6 @@ void LSM303D::measureAccelerometer() { perf_begin(_accel_sample_perf); - perf_count(_accel_sample_interval_perf); // status register and data as read back from the device #pragma pack(push, 1) @@ -552,7 +547,6 @@ void LSM303D::measureMagnetometer() { perf_begin(_mag_sample_perf); - perf_count(_mag_sample_interval_perf); // status register and data as read back from the device #pragma pack(push, 1) @@ -591,9 +585,7 @@ void LSM303D::print_info() { perf_print_counter(_accel_sample_perf); - perf_print_counter(_accel_sample_interval_perf); perf_print_counter(_mag_sample_perf); - perf_print_counter(_mag_sample_interval_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); perf_print_counter(_accel_duplicates); diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp index 819b162787..f8a7a9c67b 100644 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -274,9 +274,7 @@ private: unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE}; perf_counter_t _accel_sample_perf; - perf_counter_t _accel_sample_interval_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _mag_sample_interval_perf; perf_counter_t _bad_registers; perf_counter_t _bad_values; perf_counter_t _accel_duplicates; diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp index 852adfa076..075ab4cb53 100644 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/mpu6000/MPU6000.cpp @@ -41,13 +41,12 @@ constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; MPU6000::MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type) : CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _device_type(device_type), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")), - _measure_interval(perf_alloc(PC_INTERVAL, "mpu6k_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6k_bad_trans")), _bad_registers(perf_alloc(PC_COUNT, "mpu6k_bad_reg")), _reset_retries(perf_alloc(PC_COUNT, "mpu6k_reset")), @@ -84,7 +83,6 @@ MPU6000::~MPU6000() /* delete the perf counter */ perf_free(_sample_perf); - perf_free(_measure_interval); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_reset_retries); @@ -729,8 +727,6 @@ MPU6000::check_registers(void) int MPU6000::measure() { - perf_count(_measure_interval); - if (_in_factory_test) { // don't publish any data while in factory test mode return OK; @@ -897,7 +893,6 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_measure_interval); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_reset_retries); diff --git a/src/drivers/imu/mpu6000/MPU6000.hpp b/src/drivers/imu/mpu6000/MPU6000.hpp index a75a1716d1..ea2a412739 100644 --- a/src/drivers/imu/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/mpu6000/MPU6000.hpp @@ -358,7 +358,6 @@ private: unsigned _sample_rate{1000}; perf_counter_t _sample_perf; - perf_counter_t _measure_interval; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _reset_retries; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index d694b0453d..d839804b31 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -72,7 +72,7 @@ const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only) : - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 5aa9f2c35b..9ee743047e 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -148,7 +148,7 @@ extern "C" __EXPORT int irlock_main(int argc, char *argv[]); /** constructor **/ IRLOCK::IRLOCK(int bus, int address) : I2C("irlock", IRLOCK0_DEVICE_PATH, bus, address, 400000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _reports(nullptr), _sensor_ok(false), _read_failures(0), diff --git a/src/drivers/lights/blinkm/blinkm.cpp b/src/drivers/lights/blinkm/blinkm.cpp index 9eb26895f5..52d99992e3 100644 --- a/src/drivers/lights/blinkm/blinkm.cpp +++ b/src/drivers/lights/blinkm/blinkm.cpp @@ -256,7 +256,7 @@ extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index 49cf847f16..ca0a534fc2 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/src/drivers/lights/rgbled/rgbled.cpp @@ -105,7 +105,7 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { } diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index b77211e35f..3926c46e03 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -105,7 +105,7 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]); RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) : I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { } diff --git a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp index f49e569d58..0004389212 100644 --- a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp @@ -88,7 +88,7 @@ RGBLED_PWM *g_rgbled = nullptr; RGBLED_PWM::RGBLED_PWM() : CDev("rgbled_pwm", RGBLED_PWM0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { } diff --git a/src/drivers/linux_sbus/linux_sbus.h b/src/drivers/linux_sbus/linux_sbus.h index e03a849fd4..8c34dd1853 100644 --- a/src/drivers/linux_sbus/linux_sbus.h +++ b/src/drivers/linux_sbus/linux_sbus.h @@ -70,7 +70,7 @@ class RcInput : public px4::ScheduledWorkItem { public: RcInput() : - ScheduledWorkItem(px4::wq_configurations::hp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _shouldExit(false), _isRunning(false), _rcinput_pub(nullptr), diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp index 413a8b91a5..9b78673ee2 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ b/src/drivers/magnetometer/ak09916/ak09916.cpp @@ -165,7 +165,7 @@ usage() // Otherwise, it will passthrough the parent AK09916 AK09916::AK09916(int bus, const char *path, enum Rotation rotation) : I2C("AK09916", path, bus, AK09916_I2C_ADDR, 400000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _px4_mag(get_device_id(), ORB_PRIO_MAX, rotation), _mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")), _mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")), diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp index cbf8ab57ef..0ab3290994 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ b/src/drivers/magnetometer/bmm150/bmm150.cpp @@ -251,7 +251,7 @@ usage() BMM150::BMM150(int bus, const char *path, enum Rotation rotation) : I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _running(false), _call_interval(0), _reports(nullptr), diff --git a/src/drivers/magnetometer/hmc5883/hmc5883.cpp b/src/drivers/magnetometer/hmc5883/hmc5883.cpp index 1a6e51211e..d9efe6725a 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883.cpp @@ -326,7 +326,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : CDev("HMC5883", path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _reports(nullptr), _scale{}, diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index 852a59437f..4bb6e1513c 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -377,7 +377,7 @@ extern "C" __EXPORT int ist8310_main(int argc, char *argv[]); IST8310::IST8310(int bus_number, int address, const char *path, enum Rotation rotation) : I2C("IST8310", path, bus_number, address, IST8310_DEFAULT_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "ist8310_read")), _comms_errors(perf_alloc(PC_COUNT, "ist8310_com_err")), _range_errors(perf_alloc(PC_COUNT, "ist8310_rng_err")), diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index ba75e887de..0fa6b8bd02 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -44,7 +44,7 @@ LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation) : CDev("LIS3MDL", path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _reports(nullptr), _scale{}, diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 86cf6524aa..2b222be336 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -63,7 +63,7 @@ static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40; LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("LSM303AGR", path, bus, device, SPIDEV_MODE3, 8 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _mag_sample_perf(perf_alloc(PC_ELAPSED, "LSM303AGR_mag_read")), _bad_registers(perf_alloc(PC_COUNT, "LSM303AGR_bad_reg")), _bad_values(perf_alloc(PC_COUNT, "LSM303AGR_bad_val")), diff --git a/src/drivers/magnetometer/qmc5883/qmc5883.cpp b/src/drivers/magnetometer/qmc5883/qmc5883.cpp index 6e3665e87e..8226daf1a4 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883.cpp +++ b/src/drivers/magnetometer/qmc5883/qmc5883.cpp @@ -261,7 +261,7 @@ extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]); QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) : CDev("QMC5883", path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _reports(nullptr), _scale{}, diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index 8a39f4a07e..8f14a489bb 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -43,7 +43,7 @@ RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotation) : CDev("RM3100", path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _reports(nullptr), _scale{}, diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 73edfca294..65a5683d6d 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -35,9 +35,8 @@ PAW3902::PAW3902(int bus, enum Rotation yaw_rotation) : SPI("PAW3902", nullptr, bus, PAW3902_SPIDEV, SPIDEV_MODE0, PAW3902_SPI_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "paw3902: read")), - _interval_perf(perf_alloc(PC_INTERVAL, "paw3902: interval")), _comms_errors(perf_alloc(PC_COUNT, "paw3902: com_err")), _dupe_count_perf(perf_alloc(PC_COUNT, "paw3902: duplicate reading")), _yaw_rotation(yaw_rotation) @@ -51,12 +50,10 @@ PAW3902::~PAW3902() // free perf counters perf_free(_sample_perf); - perf_free(_interval_perf); perf_free(_comms_errors); perf_free(_dupe_count_perf); } - int PAW3902::init() { @@ -540,7 +537,6 @@ void PAW3902::Run() { perf_begin(_sample_perf); - perf_count(_interval_perf); struct TransferBuffer { uint8_t cmd = Register::Motion_Burst; @@ -699,7 +695,6 @@ void PAW3902::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_interval_perf); perf_print_counter(_comms_errors); perf_print_counter(_dupe_count_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 8dd56eb11b..14f4ff3647 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -120,7 +120,6 @@ private: uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; perf_counter_t _sample_perf; - perf_counter_t _interval_perf; perf_counter_t _comms_errors; perf_counter_t _dupe_count_perf; diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index 1bdc45cd2e..f282c962f9 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -37,7 +37,7 @@ static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")), _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")), _yaw_rotation(yaw_rotation) diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index e65f69465f..55e5d40db1 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -153,7 +153,7 @@ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation, int conversion_interval, uint8_t sonar_rotation) : I2C("PX4FLOW", PX4FLOW0_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sonar_rotation(sonar_rotation), _sample_perf(perf_alloc(PC_ELAPSED, "px4f_read")), _comms_errors(perf_alloc(PC_COUNT, "px4f_com_err")), diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 2e4d408ed9..d8a809cba2 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -49,7 +49,7 @@ static constexpr uint32_t OSD_UPDATE_RATE{500_ms}; // 2 Hz OSDatxxxx::OSDatxxxx(int bus) : SPI("OSD", nullptr, bus, PX4_MK_SPI_SEL(bus, OSD_SPIDEV), SPIDEV_MODE0, OSD_SPI_BUS_SPEED), ModuleParams(nullptr), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { } diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 6b3ef7307e..4e30536658 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -184,7 +184,7 @@ extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); PCA9685::PCA9685(int bus, uint8_t address) : I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _mode(IOX_MODE_OFF), _running(false), _i2cpwm_interval(1_s / 60.0f), diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index b1e740abee..1b23e888d2 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -222,7 +222,7 @@ extern "C" __EXPORT int ina226_main(int argc, char *argv[]); INA226::INA226(int bus, int address) : I2C("INA226", nullptr, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")) { diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 89246ee2e8..9ffef8e70f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include #include #include @@ -215,7 +214,7 @@ private: PX4FMU::PX4FMU() : CDev(PX4FMU_DEVICE_PATH), - OutputModuleInterface(px4::wq_configurations::hp_default), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")), _cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval")) { diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index b3ad8116bf..a0d658b795 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -45,8 +45,8 @@ work_s RCInput::_work = {}; constexpr char const *RCInput::RC_SCAN_STRING[]; RCInput::RCInput(bool run_as_task, char *device) : - _cycle_perf(perf_alloc(PC_ELAPSED, "rc_input cycle time")), - _publish_interval_perf(perf_alloc(PC_INTERVAL, "rc_input publish interval")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), + _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) { // rc input, published to ORB _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.h b/src/drivers/rpi_rc_in/rpi_rc_in.h index 2398cde842..7d3766bbaa 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.h +++ b/src/drivers/rpi_rc_in/rpi_rc_in.h @@ -64,7 +64,7 @@ namespace rpi_rc_in class RcInput : public px4::ScheduledWorkItem { public: - RcInput() : ScheduledWorkItem(px4::wq_configurations::hp_default) {} + RcInput() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {} ~RcInput(); diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 0cab2b385f..ae3f1975b9 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -50,7 +50,7 @@ class SafetyButton : public ModuleBase, public px4::ScheduledWorkItem { public: - SafetyButton() : ScheduledWorkItem(px4::wq_configurations::hp_default) {} + SafetyButton() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {} virtual ~SafetyButton(); /** @see ModuleBase */ diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 645592afd9..fdf44f1354 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -191,7 +191,7 @@ static BST *g_bst = nullptr; BST::BST(int bus) : I2C("bst", nullptr, bus, BST_ADDR, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) { } diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index bd5d12c6a8..15f5cf8078 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -41,7 +41,7 @@ ToneAlarm::ToneAlarm() : CDev(TONE_ALARM0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { } diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp index 0997d931ee..6085c39646 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/lib/drivers/airspeed/airspeed.cpp @@ -58,7 +58,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), _sensor_ok(false), _measure_interval(0), _collect_phase(false), diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 9419f8ee20..e451b22b31 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -32,4 +32,3 @@ ############################################################################ px4_add_library(mixer_module mixer_module.cpp) - diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 55d9b80141..c94848da24 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -524,4 +524,3 @@ int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len) return _command.result; } - diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 63d23a60fe..e60a2847ad 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -61,8 +61,8 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams public: static constexpr int MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; - OutputModuleInterface(const px4::wq_config_t &config) - : px4::ScheduledWorkItem(config), ModuleParams(nullptr) {} + OutputModuleInterface(const char *name, const px4::wq_config_t &config) + : px4::ScheduledWorkItem(name, config), ModuleParams(nullptr) {} virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; @@ -239,4 +239,3 @@ private: ) }; - diff --git a/src/lib/output_limit/output_limit.h b/src/lib/output_limit/output_limit.h index be9d969bd1..e240ae6a19 100644 --- a/src/lib/output_limit/output_limit.h +++ b/src/lib/output_limit/output_limit.h @@ -77,4 +77,3 @@ __EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const un const float *output, uint16_t *effective_output, output_limit_t *limit); __END_DECLS - diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 87adfef289..80044ae8a0 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -137,8 +137,6 @@ private: bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */ perf_counter_t _perf_elapsed{}; - perf_counter_t _perf_interval{}; - DEFINE_PARAMETERS( (ParamFloat) _param_west_w_p_noise, @@ -170,13 +168,12 @@ private: AirspeedModule::AirspeedModule(): ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { // initialise parameters update_params(); _perf_elapsed = perf_alloc_once(PC_ELAPSED, "airspeed_selector elapsed"); - _perf_interval = perf_alloc_once(PC_INTERVAL, "airspeed_selector interval"); } AirspeedModule::~AirspeedModule() @@ -184,7 +181,6 @@ AirspeedModule::~AirspeedModule() ScheduleClear(); perf_free(_perf_elapsed); - perf_free(_perf_interval); if (_airspeed_validator != nullptr) { delete[] _airspeed_validator; @@ -216,7 +212,6 @@ AirspeedModule::task_spawn(int argc, char *argv[]) void AirspeedModule::Run() { - perf_count(_perf_interval); perf_begin(_perf_elapsed); /* the first time we run through here, initialize N airspeedValidator @@ -572,7 +567,6 @@ and also publishes those. int AirspeedModule::print_status() { perf_print_counter(_perf_elapsed); - perf_print_counter(_perf_interval); int instance = 0; uORB::SubscriptionData est{ORB_ID(airspeed_validated), (uint8_t)instance}; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 8b25362a9f..18da29cf40 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -45,9 +45,8 @@ using namespace time_literals; extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); FixedwingAttitudeControl::FixedwingAttitudeControl() : - WorkItem(px4::wq_configurations::att_pos_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")), - _loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_att_control: interval")) + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")) { // check if VTOL first vehicle_status_poll(); @@ -132,7 +131,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : FixedwingAttitudeControl::~FixedwingAttitudeControl() { perf_free(_loop_perf); - perf_free(_loop_interval_perf); } bool @@ -450,7 +448,6 @@ void FixedwingAttitudeControl::Run() } perf_begin(_loop_perf); - perf_count(_loop_interval_perf); if (_att_sub.update(&_att)) { @@ -922,7 +919,6 @@ int FixedwingAttitudeControl::print_status() PX4_INFO("Running"); perf_print_counter(_loop_perf); - perf_print_counter(_loop_interval_perf); return 0; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index e33369cf5b..fb837e896d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -131,7 +131,6 @@ private: vehicle_status_s _vehicle_status {}; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _loop_interval_perf; /**< loop interval performance counter */ float _flaps_applied{0.0f}; float _flaperons_applied{0.0f}; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1c53832d6a..d419e97a77 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -37,9 +37,8 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); FixedwingPositionControl::FixedwingPositionControl() : ModuleParams(nullptr), - WorkItem(px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "fw_pos_control_l1: cycle")), - _loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_pos_control_l1: interval")), _launchDetector(this), _runway_takeoff(this) { @@ -114,7 +113,6 @@ FixedwingPositionControl::FixedwingPositionControl() : FixedwingPositionControl::~FixedwingPositionControl() { perf_free(_loop_perf); - perf_free(_loop_interval_perf); } bool @@ -1657,7 +1655,6 @@ FixedwingPositionControl::Run() } perf_begin(_loop_perf); - perf_count(_loop_interval_perf); /* only run controller if position changed */ if (_global_pos_sub.update(&_global_pos)) { @@ -1990,7 +1987,6 @@ int FixedwingPositionControl::print_status() PX4_INFO("Running"); perf_print_counter(_loop_perf); - perf_print_counter(_loop_interval_perf); return 0; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 2cec57b5ed..1dc9fce20e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -187,7 +187,6 @@ private: SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; ///< loop performance counter */ - perf_counter_t _loop_interval_perf; ///< loop interval performance counter */ float _hold_alt{0.0f}; ///< hold altitude for altitude mode */ float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched */ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index e6e2a7d2ea..1c76d051cb 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -55,7 +55,7 @@ namespace land_detector LandDetector::LandDetector() : ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { _land_detected.timestamp = hrt_absolute_time(); _land_detected.freefall = false; diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp index 7f4e10b0de..a8670a8398 100644 --- a/src/modules/load_mon/load_mon.cpp +++ b/src/modules/load_mon/load_mon.cpp @@ -123,7 +123,7 @@ private: LoadMon::LoadMon() : ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::lp_default), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), _stack_perf(perf_alloc(PC_ELAPSED, "stack_check")) { } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index fa265e9683..87786dd089 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,7 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl() : ModuleParams(nullptr), - WorkItem(px4::wq_configurations::rate_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index b9f845434e..d977b7ed81 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -53,6 +53,6 @@ px4_add_module( git_ecl ecl_validation mathlib - sensors__vehicle_acceleration - sensors__vehicle_angular_velocity + vehicle_acceleration + vehicle_angular_velocity ) diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt index 337f659c46..812abb7cd6 100644 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -px4_add_library(sensors__vehicle_acceleration +px4_add_library(vehicle_acceleration VehicleAcceleration.cpp ) -target_link_libraries(sensors__vehicle_acceleration PRIVATE px4_work_queue) +target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 8c66d6ae00..0fd7573333 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -40,9 +40,8 @@ using namespace time_literals; VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), - WorkItem(px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")), - _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_acceleration: interval")), _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency")) { } @@ -52,7 +51,6 @@ VehicleAcceleration::~VehicleAcceleration() Stop(); perf_free(_cycle_perf); - perf_free(_interval_perf); perf_free(_sensor_latency_perf); } @@ -178,7 +176,6 @@ void VehicleAcceleration::Run() { perf_begin(_cycle_perf); - perf_count(_interval_perf); // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); @@ -220,6 +217,5 @@ VehicleAcceleration::PrintStatus() PX4_INFO("selected sensor: %d", _selected_sensor); perf_print_counter(_cycle_perf); - perf_print_counter(_interval_perf); perf_print_counter(_sensor_latency_perf); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index f84cef18ad..9cd4c9d496 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -102,7 +102,6 @@ private: matrix::Vector3f _bias; perf_counter_t _cycle_perf; - perf_counter_t _interval_perf; perf_counter_t _sensor_latency_perf; uint8_t _selected_sensor{0}; diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt index 58aa0d92e3..340252aa99 100644 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -px4_add_library(sensors__vehicle_angular_velocity +px4_add_library(vehicle_angular_velocity VehicleAngularVelocity.cpp ) -target_link_libraries(sensors__vehicle_angular_velocity PRIVATE px4_work_queue) +target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index d84ec16eb6..09228c2d07 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -40,9 +40,8 @@ using namespace time_literals; VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), - WorkItem(px4::wq_configurations::rate_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), - _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")), _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency")) { } @@ -52,7 +51,6 @@ VehicleAngularVelocity::~VehicleAngularVelocity() Stop(); perf_free(_cycle_perf); - perf_free(_interval_perf); perf_free(_sensor_latency_perf); } @@ -217,7 +215,6 @@ void VehicleAngularVelocity::Run() { perf_begin(_cycle_perf); - perf_count(_interval_perf); // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); @@ -296,6 +293,5 @@ VehicleAngularVelocity::PrintStatus() } perf_print_counter(_cycle_perf); - perf_print_counter(_interval_perf); perf_print_counter(_sensor_latency_perf); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 81ee9fff84..4269ba5775 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -110,7 +110,6 @@ private: matrix::Vector3f _bias; perf_counter_t _cycle_perf; - perf_counter_t _interval_perf; perf_counter_t _sensor_latency_perf; uint32_t _selected_sensor_device_id{0}; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 49683cccf2..3facf8f049 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -54,9 +54,8 @@ using namespace matrix; VtolAttitudeControl::VtolAttitudeControl() : - WorkItem(px4::wq_configurations::rate_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")), - _loop_interval_perf(perf_alloc(PC_INTERVAL, "vtol_att_control: interval")) + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")) { _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -108,7 +107,6 @@ VtolAttitudeControl::VtolAttitudeControl() : VtolAttitudeControl::~VtolAttitudeControl() { perf_free(_loop_perf); - perf_free(_loop_interval_perf); } bool @@ -309,7 +307,6 @@ VtolAttitudeControl::Run() } perf_begin(_loop_perf); - perf_count(_loop_interval_perf); const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in); @@ -513,7 +510,6 @@ VtolAttitudeControl::print_status() PX4_INFO("Running"); perf_print_counter(_loop_perf); - perf_print_counter(_loop_interval_perf); return PX4_OK; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2e9a81acaf..82a3505731 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -217,7 +217,6 @@ private: bool _initialized{false}; perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _loop_interval_perf; /**< loop interval performance counter */ void vehicle_cmd_poll(); diff --git a/src/systemcmds/work_queue/CMakeLists.txt b/src/systemcmds/work_queue/CMakeLists.txt new file mode 100644 index 0000000000..0ef8d88c60 --- /dev/null +++ b/src/systemcmds/work_queue/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__work_queue + MAIN work_queue + SRCS + work_queue_main.cpp + ) diff --git a/src/systemcmds/work_queue/work_queue_main.cpp b/src/systemcmds/work_queue/work_queue_main.cpp new file mode 100644 index 0000000000..0f6fdcfcf2 --- /dev/null +++ b/src/systemcmds/work_queue/work_queue_main.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +static void usage(); + +extern "C" { + __EXPORT int work_queue_main(int argc, char *argv[]); +} + +int +work_queue_main(int argc, char *argv[]) +{ + if (argc != 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + px4::WorkQueueManagerStart(); + return 0; + + } else if (!strcmp(argv[1], "stop")) { + px4::WorkQueueManagerStop(); + return 0; + + } else if (!strcmp(argv[1], "status")) { + px4::WorkQueueManagerStatus(); + return 0; + } + + usage(); + + return 0; +} + +static void +usage() +{ + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Command-line tool to show work queue status. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("work_queue", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +}