From 3c49eae8d44c7223ade3ea902680709c0821d3b6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 28 Nov 2018 22:15:29 -0500 Subject: [PATCH] PX4 general work queue --- .ci/Jenkinsfile-compile | 2 +- platforms/nuttx/src/px4_layer/CMakeLists.txt | 1 + platforms/nuttx/src/px4_layer/px4_init.cpp | 3 + platforms/posix/CMakeLists.txt | 7 +- platforms/posix/src/px4_layer/CMakeLists.txt | 2 +- platforms/posix/src/px4_layer/px4_init.cpp | 3 + platforms/qurt/src/px4_layer/px4_init.cpp | 3 + posix-configs/SITL/init/test/cmd_template.in | 2 +- src/include/containers/BlockingList.hpp | 90 ++++++ src/include/containers/BlockingQueue.hpp | 89 ++++++ src/include/containers/List.hpp | 6 +- src/include/containers/LockGuard.hpp | 54 ++++ src/platforms/common/CMakeLists.txt | 1 + .../common/px4_work_queue/CMakeLists.txt | 45 +++ .../px4_work_queue/ScheduledWorkItem.cpp | 65 ++++ .../px4_work_queue/ScheduledWorkItem.hpp | 80 +++++ .../common/px4_work_queue/WorkItem.cpp | 67 ++++ .../common/px4_work_queue/WorkItem.hpp | 70 +++++ .../common/px4_work_queue/WorkQueue.cpp | 110 +++++++ .../common/px4_work_queue/WorkQueue.hpp | 95 ++++++ .../px4_work_queue/WorkQueueManager.cpp | 289 ++++++++++++++++++ .../px4_work_queue/WorkQueueManager.hpp | 100 ++++++ .../common/px4_work_queue/test/CMakeLists.txt | 44 +++ .../px4_work_queue/test/wqueue_main.cpp | 57 ++++ .../test/wqueue_scheduled_test.cpp | 80 +++++ .../test/wqueue_scheduled_test.h | 56 ++++ .../px4_work_queue/test/wqueue_start.cpp | 90 ++++++ .../px4_work_queue/test/wqueue_test.cpp | 83 +++++ .../common/px4_work_queue/test/wqueue_test.h | 56 ++++ src/platforms/px4_tasks.h | 17 +- 30 files changed, 1651 insertions(+), 16 deletions(-) create mode 100644 src/include/containers/BlockingList.hpp create mode 100644 src/include/containers/BlockingQueue.hpp create mode 100644 src/include/containers/LockGuard.hpp create mode 100644 src/platforms/common/px4_work_queue/CMakeLists.txt create mode 100644 src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp create mode 100644 src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp create mode 100644 src/platforms/common/px4_work_queue/WorkItem.cpp create mode 100644 src/platforms/common/px4_work_queue/WorkItem.hpp create mode 100644 src/platforms/common/px4_work_queue/WorkQueue.cpp create mode 100644 src/platforms/common/px4_work_queue/WorkQueue.hpp create mode 100644 src/platforms/common/px4_work_queue/WorkQueueManager.cpp create mode 100644 src/platforms/common/px4_work_queue/WorkQueueManager.hpp create mode 100644 src/platforms/common/px4_work_queue/test/CMakeLists.txt create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_main.cpp create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_start.cpp create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_test.cpp create mode 100644 src/platforms/common/px4_work_queue/test/wqueue_test.h diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 27370b47dc47..3c985f57d611 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -43,7 +43,7 @@ pipeline { ] def nuttx_builds_other = [ - target: ["px4_cannode-v1_default", "px4_esc-v1_default", "thiemar_s2740vc-v1_default"], + target: ["px4_esc-v1_default", "thiemar_s2740vc-v1_default"], image: docker_images.nuttx, archive: false ] diff --git a/platforms/nuttx/src/px4_layer/CMakeLists.txt b/platforms/nuttx/src/px4_layer/CMakeLists.txt index 6771b9ba2793..301317932411 100644 --- a/platforms/nuttx/src/px4_layer/CMakeLists.txt +++ b/platforms/nuttx/src/px4_layer/CMakeLists.txt @@ -44,6 +44,7 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io") nuttx_apps # up_cxxinitialize nuttx_sched drivers_boards_common_arch + px4_work_queue ) else() add_library(px4_layer ${PX4_SOURCE_DIR}/src/platforms/empty.c) diff --git a/platforms/nuttx/src/px4_layer/px4_init.cpp b/platforms/nuttx/src/px4_layer/px4_init.cpp index 53950c849202..b8451c774588 100644 --- a/platforms/nuttx/src/px4_layer/px4_init.cpp +++ b/platforms/nuttx/src/px4_layer/px4_init.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -104,5 +105,7 @@ int px4_platform_init(void) cpuload_initialize_once(); #endif + px4::WorkQueueManagerStart(); + return PX4_OK; } diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 963c3639bd91..3ba23f6123ea 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -62,10 +62,11 @@ else() ${df_driver_libs} df_driver_framework pthread m - + # horrible circular dependencies that need to be teased apart - px4_layer - px4_platform + px4_layer px4_platform + work_queue + parameters ) if (NOT APPLE) diff --git a/platforms/posix/src/px4_layer/CMakeLists.txt b/platforms/posix/src/px4_layer/CMakeLists.txt index 15db5dd86933..ffa06384614d 100644 --- a/platforms/posix/src/px4_layer/CMakeLists.txt +++ b/platforms/posix/src/px4_layer/CMakeLists.txt @@ -55,7 +55,7 @@ add_library(px4_layer ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable -target_link_libraries(px4_layer PRIVATE work_queue) +target_link_libraries(px4_layer PRIVATE work_queue px4_work_queue) target_link_libraries(px4_layer PRIVATE px4_daemon) if(ENABLE_LOCKSTEP_SCHEDULER) diff --git a/platforms/posix/src/px4_layer/px4_init.cpp b/platforms/posix/src/px4_layer/px4_init.cpp index 6689420fec05..e162ed15cd1b 100644 --- a/platforms/posix/src/px4_layer/px4_init.cpp +++ b/platforms/posix/src/px4_layer/px4_init.cpp @@ -37,6 +37,7 @@ #include #include #include +#include int px4_platform_init(void) { @@ -44,5 +45,7 @@ int px4_platform_init(void) param_init(); + px4::WorkQueueManagerStart(); + return PX4_OK; } diff --git a/platforms/qurt/src/px4_layer/px4_init.cpp b/platforms/qurt/src/px4_layer/px4_init.cpp index 6689420fec05..e162ed15cd1b 100644 --- a/platforms/qurt/src/px4_layer/px4_init.cpp +++ b/platforms/qurt/src/px4_layer/px4_init.cpp @@ -37,6 +37,7 @@ #include #include #include +#include int px4_platform_init(void) { @@ -44,5 +45,7 @@ int px4_platform_init(void) param_init(); + px4::WorkQueueManagerStart(); + return PX4_OK; } diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index d5f06d72c60b..a16fcabd1aa8 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -25,7 +25,7 @@ mavlink start -x -u 14556 -r 2000000 mavlink boot_complete @cmd_name@ start -sleep 1 +sleep 3 @cmd_name@ start sleep 1 @cmd_name@ stop diff --git a/src/include/containers/BlockingList.hpp b/src/include/containers/BlockingList.hpp new file mode 100644 index 000000000000..27e3f896cf4d --- /dev/null +++ b/src/include/containers/BlockingList.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file BlockingList.hpp + * + * A blocking intrusive linked list. + */ + +#pragma once + +#include "List.hpp" +#include "LockGuard.hpp" + +#include +#include + +template +class BlockingList : public List +{ +public: + + ~BlockingList() + { + pthread_mutex_destroy(&_mutex); + pthread_cond_destroy(&_cv); + } + + void add(T newNode) + { + LockGuard lg{_mutex}; + List::add(newNode); + } + + bool remove(T removeNode) + { + LockGuard lg{_mutex}; + return List::remove(removeNode); + } + + size_t size() const + { + LockGuard lg{_mutex}; + return List::size(); + } + + void clear() + { + LockGuard lg{_mutex}; + List::clear(); + } + + LockGuard getLockGuard() { return LockGuard{_mutex}; } + +private: + + pthread_mutex_t _mutex = PTHREAD_MUTEX_INITIALIZER; + pthread_cond_t _cv = PTHREAD_COND_INITIALIZER; + +}; diff --git a/src/include/containers/BlockingQueue.hpp b/src/include/containers/BlockingQueue.hpp new file mode 100644 index 000000000000..7e0ac7f89e0a --- /dev/null +++ b/src/include/containers/BlockingQueue.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "LockGuard.hpp" + +template +class BlockingQueue +{ +public: + + BlockingQueue() + { + px4_sem_init(&_sem_head, 0, N); + px4_sem_init(&_sem_tail, 0, 0); + } + + ~BlockingQueue() + { + px4_sem_destroy(&_sem_head); + px4_sem_destroy(&_sem_tail); + } + + void push(T newItem) + { + px4_sem_wait(&_sem_head); + + _data[_tail] = newItem; + _tail = (_tail + 1) % N; + + px4_sem_post(&_sem_tail); + } + + T pop() + { + px4_sem_wait(&_sem_tail); + + T ret = _data[_head]; + _head = (_head + 1) % N; + + px4_sem_post(&_sem_head); + + return ret; + } + +private: + + px4_sem_t _sem_head; + px4_sem_t _sem_tail; + + T _data[N]; + + size_t _head{0}; + size_t _tail{0}; + +}; diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index bfd875e491c7..c005cf17a4a7 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -46,12 +46,12 @@ class ListNode { public: - void setSibling(T sibling) { _sibling = sibling; } - const T getSibling() const { return _sibling; } + void setSibling(T sibling) { _list_node_sibling = sibling; } + const T getSibling() const { return _list_node_sibling; } protected: - T _sibling{nullptr}; + T _list_node_sibling{nullptr}; }; diff --git a/src/include/containers/LockGuard.hpp b/src/include/containers/LockGuard.hpp new file mode 100644 index 000000000000..b5528d1b00db --- /dev/null +++ b/src/include/containers/LockGuard.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +class LockGuard +{ +public: + explicit LockGuard(pthread_mutex_t &mutex) : + _mutex(mutex) + { + pthread_mutex_lock(&_mutex); + } + + ~LockGuard() + { + pthread_mutex_unlock(&_mutex); + } + +private: + pthread_mutex_t &_mutex; +}; diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index afffe9fb3413..9a5f459b4e78 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -52,4 +52,5 @@ if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2") target_link_libraries(px4_platform PRIVATE modules__uORB) # px4_log awkward dependency with uORB, TODO: orb should part of the platform layer endif() +add_subdirectory(px4_work_queue) add_subdirectory(work_queue) diff --git a/src/platforms/common/px4_work_queue/CMakeLists.txt b/src/platforms/common/px4_work_queue/CMakeLists.txt new file mode 100644 index 000000000000..bcbf7f6bb499 --- /dev/null +++ b/src/platforms/common/px4_work_queue/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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_library(px4_work_queue + ScheduledWorkItem.cpp + WorkItem.cpp + WorkQueue.cpp + WorkQueueManager.cpp +) + +if(PX4_TESTING) + add_subdirectory(test) +endif() + +target_link_libraries(px4_work_queue PRIVATE px4_platform) diff --git a/src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp new file mode 100644 index 000000000000..d729e241340f --- /dev/null +++ b/src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 "ScheduledWorkItem.hpp" + +namespace px4 +{ + +ScheduledWorkItem::~ScheduledWorkItem() +{ + ScheduleClear(); +} + +void ScheduledWorkItem::schedule_trampoline(void *arg) +{ + ScheduledWorkItem *dev = reinterpret_cast(arg); + dev->ScheduleNow(); +} + +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) +{ + hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); +} + +void ScheduledWorkItem::ScheduleClear() +{ + hrt_cancel(&_call); +} + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp b/src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp new file mode 100644 index 000000000000..7bcd39f7c94c --- /dev/null +++ b/src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "WorkItem.hpp" + +#include + +namespace px4 +{ + +class ScheduledWorkItem : public WorkItem +{ +public: + + ScheduledWorkItem(const wq_config_t &config) : WorkItem(config) {} + virtual ~ScheduledWorkItem() override; + + /** + * Schedule next run with a delay in microseconds. + * + * @param delay_us The delay in microseconds. + */ + void ScheduleDelayed(uint32_t delay_us); + + /** + * Schedule repeating run with optional delay. + * + * @param interval_us The interval in microseconds. + * @param delay_us The delay (optional) in microseconds. + */ + void ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us = 0); + + /** + * Clear any scheduled work. + */ + void ScheduleClear(); + + virtual void Run() override = 0; + +private: + + static void schedule_trampoline(void *arg); + + hrt_call _call{}; + +}; + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkItem.cpp b/src/platforms/common/px4_work_queue/WorkItem.cpp new file mode 100644 index 000000000000..ec28e2e236d5 --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkItem.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 "WorkItem.hpp" + +#include "WorkQueue.hpp" +#include "WorkQueueManager.hpp" + +#include +#include + +namespace px4 +{ + +WorkItem::WorkItem(const wq_config_t &config) +{ + if (!Init(config)) { + PX4_ERR("init failed"); + } +} + +bool WorkItem::Init(const wq_config_t &config) +{ + px4::WorkQueue *wq = WorkQueueFindOrCreate(config); + + if (wq == nullptr) { + PX4_ERR("%s not available", config.name); + + } else { + _wq = wq; + return true; + } + + return false; +} + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkItem.hpp b/src/platforms/common/px4_work_queue/WorkItem.hpp new file mode 100644 index 000000000000..a71cd05c3848 --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkItem.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + + +#include "WorkQueueManager.hpp" +#include "WorkQueue.hpp" + +#include +#include +#include + +namespace px4 +{ + +class WorkItem : public IntrusiveQueueNode +{ +public: + + explicit WorkItem(const wq_config_t &config); + WorkItem() = delete; + + virtual ~WorkItem() = default; + + inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); } + + virtual void Run() = 0; + +protected: + + bool Init(const wq_config_t &config); + +private: + + WorkQueue *_wq{nullptr}; + +}; + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkQueue.cpp b/src/platforms/common/px4_work_queue/WorkQueue.cpp new file mode 100644 index 000000000000..965747475618 --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkQueue.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 "WorkQueue.hpp" +#include "WorkItem.hpp" + +#include + +#include +#include +#include + +namespace px4 +{ + +WorkQueue::WorkQueue(const wq_config_t &config) : + _config(config) +{ + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np(_config.name); +#elif !defined(__PX4_QURT) + pthread_setname_np(pthread_self(), _config.name); +#endif + +#ifndef __PX4_NUTTX + px4_sem_init(&_qlock, 0, 1); +#endif /* __PX4_NUTTX */ + + px4_sem_init(&_process_lock, 0, 0); + px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE); +} + +WorkQueue::~WorkQueue() +{ + work_lock(); + px4_sem_destroy(&_process_lock); + work_unlock(); + +#ifndef __PX4_NUTTX + px4_sem_destroy(&_qlock); +#endif /* __PX4_NUTTX */ +} + +void WorkQueue::Add(WorkItem *item) +{ + work_lock(); + _q.push(item); + work_unlock(); + + // Wake up the worker thread + px4_sem_post(&_process_lock); +} + +void WorkQueue::Run() +{ + while (!should_exit()) { + px4_sem_wait(&_process_lock); + + work_lock(); + + // process queued work + while (!_q.empty()) { + WorkItem *work = _q.pop(); + + work_unlock(); // unlock work queue to run (item may requeue itself) + work->Run(); + work_lock(); // re-lock + } + + work_unlock(); + } +} + +void WorkQueue::print_status() +{ + PX4_INFO("WorkQueue: %s running", get_name()); +} + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkQueue.hpp b/src/platforms/common/px4_work_queue/WorkQueue.hpp new file mode 100644 index 000000000000..dcfc57c2b688 --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkQueue.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "WorkQueueManager.hpp" + +#include +#include +#include +#include +#include +#include + +namespace px4 +{ + +class WorkItem; + +class WorkQueue : public ListNode +{ +public: + explicit WorkQueue(const wq_config_t &wq_config); + WorkQueue() = delete; + + ~WorkQueue(); + + const char *get_name() { return _config.name; } + + void Add(WorkItem *item); + + // TODO: need helpers to handle clean shutdown - remove and clear? + //void remove(WorkItem *item); + //void clear(); + + void Run(); + + void request_stop() { _should_exit.store(true); } + + void print_status(); + +private: + + bool should_exit() const { return _should_exit.load(); } + +#ifdef __PX4_NUTTX + // In NuttX work can be enqueued from an ISR + void work_lock() { _flags = enter_critical_section(); } + void work_unlock() { leave_critical_section(_flags); } + irqstate_t _flags; +#else + void work_lock() { px4_sem_wait(&_qlock); } + void work_unlock() { px4_sem_post(&_qlock); } + px4_sem_t _qlock; +#endif + + IntrusiveQueue _q; + px4_sem_t _process_lock; + + px4::atomic_bool _should_exit{false}; + const wq_config_t &_config; + +}; + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.cpp b/src/platforms/common/px4_work_queue/WorkQueueManager.cpp new file mode 100644 index 000000000000..9b15bb2c774d --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * 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 "WorkQueueManager.hpp" + +#include "WorkQueue.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace time_literals; + +namespace px4 +{ + +// list of current work queues +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 WorkQueue *FindWorkQueueByName(const char *name) +{ + if (_wq_manager_wqs_list == nullptr) { + PX4_ERR("not running"); + return nullptr; + } + + auto lg = _wq_manager_wqs_list->getLockGuard(); + + // search list + for (WorkQueue *wq : *_wq_manager_wqs_list) { + if (strcmp(wq->get_name(), name) == 0) { + return wq; + } + } + + return nullptr; +} + +WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq) +{ + if (_wq_manager_create_queue == nullptr) { + PX4_ERR("not running"); + return nullptr; + } + + // search list for existing work queue + WorkQueue *wq = FindWorkQueueByName(new_wq.name); + + // create work queue if it doesn't already exist + if (wq == nullptr) { + // add WQ config to list + // main thread wakes up, creates the thread + _wq_manager_create_queue->push(&new_wq); + + // we wait until new wq is created, then return + uint64_t t = 0; + + while (wq == nullptr && t < 10_s) { + // Wait up to 10 seconds, checking every 1 ms + t += 1_ms; + px4_usleep(1_ms); + + wq = FindWorkQueueByName(new_wq.name); + } + + if (wq == nullptr) { + PX4_ERR("failed to create %s", new_wq.name); + } + } + + return wq; +} + +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; + + const device::Device::DeviceBusType bus_type = device_id.devid_s.bus_type; + const uint8_t bus = device_id.devid_s.bus; + + if (bus_type == device::Device::DeviceBusType_I2C) { + switch (bus) { + case 1: return wq_configurations::I2C1; + + case 2: return wq_configurations::I2C2; + + case 3: return wq_configurations::I2C3; + + case 4: return wq_configurations::I2C4; + } + + } else if (bus_type == device::Device::DeviceBusType_SPI) { + switch (bus) { + case 1: return wq_configurations::SPI1; + + case 2: return wq_configurations::SPI2; + + case 3: return wq_configurations::SPI3; + + case 4: return wq_configurations::SPI4; + + case 5: return wq_configurations::SPI5; + + case 6: return wq_configurations::SPI6; + } + } + + // otherwise use high priority + return wq_configurations::hp_default; +}; + +static void *WorkQueueRunner(void *context) +{ + wq_config_t *config = static_cast(context); + WorkQueue wq(*config); + + // add to work queue list + _wq_manager_wqs_list->add(&wq); + + wq.Run(); + + // remove from work queue list + _wq_manager_wqs_list->remove(&wq); + + return nullptr; +} + +static void WorkQueueManagerRun() +{ + _wq_manager_wqs_list = new BlockingList(); + _wq_manager_create_queue = new BlockingQueue(); + + while (!_wq_manager_should_exit.load()) { + // create new work queues as needed + const wq_config_t *wq = _wq_manager_create_queue->pop(); + + if (wq != nullptr) { + // create new work queue + + pthread_attr_t attr; + int ret_attr_init = pthread_attr_init(&attr); + + if (ret_attr_init != 0) { + PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init); + } + + sched_param param; + int ret_getschedparam = pthread_attr_getschedparam(&attr, ¶m); + + if (ret_getschedparam != 0) { + PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); + } + + // stack size + const size_t stacksize = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize)); + int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize); + + if (ret_setstacksize != 0) { + PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize); + } + + // schedule policy FIFO + int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + + if (ret_setschedpolicy != 0) { + PX4_ERR("failed to set sched policy SCHED_FIFO (%i)", ret_setschedpolicy); + } + + // priority + param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority; + int ret_setschedparam = pthread_attr_setschedparam(&attr, ¶m); + + if (ret_setschedparam != 0) { + PX4_ERR("setting sched params for %s failed (%i)", wq->name, ret_setschedparam); + } + + // create thread + pthread_t thread; + 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); + + } else { + PX4_ERR("failed to create thread for %s (%i): %s", wq->name, ret_create, strerror(ret_create)); + } + + // destroy thread attributes + int ret_destroy = pthread_attr_destroy(&attr); + + if (ret_destroy != 0) { + PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create); + } + } + } +} + +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; + } + + return 0; +} + +int WorkQueueManagerStop() +{ + 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(); + } + + 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(1000); + + delete _wq_manager_create_queue; + } + + return PX4_OK; +} + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp new file mode 100644 index 000000000000..1a1f59d7d9fb --- /dev/null +++ b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +namespace px4 +{ + +class WorkQueue; // forward declaration + +struct wq_config_t { + const char *name; + uint16_t stacksize; + int8_t relative_priority; // relative to max +}; + +namespace wq_configurations +{ +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1500, 0}; // PX4 inner loop highest priority + +static constexpr wq_config_t SPI1{"wq:SPI1", 1200, -1}; +static constexpr wq_config_t SPI2{"wq:SPI2", 1200, -2}; +static constexpr wq_config_t SPI3{"wq:SPI3", 1200, -3}; +static constexpr wq_config_t SPI4{"wq:SPI4", 1200, -4}; +static constexpr wq_config_t SPI5{"wq:SPI5", 1200, -5}; +static constexpr wq_config_t SPI6{"wq:SPI6", 1200, -6}; + +static constexpr wq_config_t I2C1{"wq:I2C1", 1000, -7}; +static constexpr wq_config_t I2C2{"wq:I2C2", 1000, -8}; +static constexpr wq_config_t I2C3{"wq:I2C3", 1000, -9}; +static constexpr wq_config_t I2C4{"wq:I2C4", 1000, -10}; + +static constexpr wq_config_t hp_default{"wq:hp_default", 1000, -11}; +static constexpr wq_config_t lp_default{"wq:lp_default", 1000, -50}; + +static constexpr wq_config_t test1{"wq:test1", 800, 0}; +static constexpr wq_config_t test2{"wq:test2", 800, 0}; + +} // namespace wq_configurations + +/** + * Start the work queue manager task. + */ +int WorkQueueManagerStart(); + +/** + * Stop the work queue manager task. + */ +int WorkQueueManagerStop(); + +/** + * Create (or find) a work queue with a particular configuration. + * + * @param new_wq The work queue configuration (see WorkQueueManager.hpp). + * @return A pointer to the WorkQueue, or nullptr on failure. + */ +WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq); + +/** + * Map a PX4 driver device id to a work queue (by sensor bus). + * + * @param device_id The PX4 driver's device id. + * @return A work queue configuration. + */ +const wq_config_t &device_bus_to_wq(uint32_t device_id); + + +} // namespace px4 diff --git a/src/platforms/common/px4_work_queue/test/CMakeLists.txt b/src/platforms/common/px4_work_queue/test/CMakeLists.txt new file mode 100644 index 000000000000..4072d97f870d --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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 lib__work_queue__test__wqueue_test + MAIN wqueue_test + SRCS + wqueue_main.cpp + wqueue_scheduled_test.cpp + wqueue_start.cpp + wqueue_test.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/platforms/common/px4_work_queue/test/wqueue_main.cpp b/src/platforms/common/px4_work_queue/test/wqueue_main.cpp new file mode 100644 index 000000000000..aeb95c622b8b --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_main.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 "wqueue_test.h" +#include "wqueue_scheduled_test.h" + +#include +#include +#include +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "wqueue_test"); + + PX4_INFO("wqueue test 1"); + WQueueTest wq1; + wq1.main(); + + PX4_INFO("wqueue test 2 (scheduled)"); + WQueueScheduledTest wq2; + wq2.main(); + + PX4_INFO("wqueue test complete, exiting"); + + return 0; +} diff --git a/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp b/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp new file mode 100644 index 000000000000..a2221d0c9203 --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 "wqueue_scheduled_test.h" + +#include +#include +#include + +#include +#include +#include + +using namespace px4; + +AppState WQueueScheduledTest::appState; + +void WQueueScheduledTest::Run() +{ + //PX4_INFO("iter: %d elapsed: %" PRId64 " us", _iter, hrt_elapsed_time(&_qtime)); + + if (_iter > 1000) { + appState.requestExit(); + } + + _iter++; +} + +int WQueueScheduledTest::main() +{ + appState.setRunning(true); + + _iter = 0; + + // Put work in the work queue + ScheduleOnInterval(4000); + + // Wait for work to finsh + while (!appState.exitRequested()) { + px4_usleep(10000); + } + + PX4_INFO("WQueueScheduledTest finished"); + + //print_status(); + + px4_sleep(2); + + return 0; +} diff --git a/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h b/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h new file mode 100644 index 000000000000..538267003325 --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +using namespace px4; + +class WQueueScheduledTest : public px4::ScheduledWorkItem +{ +public: + WQueueScheduledTest() : px4::ScheduledWorkItem(px4::wq_configurations::test2) {} + ~WQueueScheduledTest() = default; + + int main(); + + void Run() override; + + static px4::AppState appState; /* track requests to terminate app */ + +private: + int _iter{0}; +}; diff --git a/src/platforms/common/px4_work_queue/test/wqueue_start.cpp b/src/platforms/common/px4_work_queue/test/wqueue_start.cpp new file mode 100644 index 000000000000..89cf64092bfc --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_start.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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 "wqueue_test.h" + +#include +#include +#include +#include +#include +#include + +static int daemon_task = -1; /* Handle of daemon task / thread */ + +extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]); +int wqueue_test_main(int argc, char *argv[]) +{ + + if (argc < 2) { + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (WQueueTest::appState.isRunning()) { + PX4_INFO("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("wqueue", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)nullptr); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + WQueueTest::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (WQueueTest::appState.isRunning()) { + PX4_INFO("is running\n"); + + } else { + PX4_INFO("not started\n"); + } + + return 0; + } + + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/common/px4_work_queue/test/wqueue_test.cpp b/src/platforms/common/px4_work_queue/test/wqueue_test.cpp new file mode 100644 index 000000000000..448ba50484ae --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_test.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 "wqueue_test.h" + +#include +#include +#include + +#include +#include +#include + +using namespace px4; + +AppState WQueueTest::appState; + +void WQueueTest::Run() +{ + //PX4_INFO("iter: %d elapsed: %" PRId64 " us", _iter, hrt_elapsed_time(&_qtime)); + + if (_iter > 10000) { + appState.requestExit(); + + } else { + ScheduleNow(); + } + + _iter++; +} + +int WQueueTest::main() +{ + appState.setRunning(true); + + _iter = 0; + + // Put work in the work queue + ScheduleNow(); + + // Wait for work to finsh + while (!appState.exitRequested()) { + px4_usleep(5000); + } + + PX4_INFO("WQueueTest finished"); + + //print_status(); + + px4_sleep(2); + + return 0; +} diff --git a/src/platforms/common/px4_work_queue/test/wqueue_test.h b/src/platforms/common/px4_work_queue/test/wqueue_test.h new file mode 100644 index 000000000000..4935901b7435 --- /dev/null +++ b/src/platforms/common/px4_work_queue/test/wqueue_test.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +using namespace px4; + +class WQueueTest : public px4::WorkItem +{ +public: + WQueueTest() : px4::WorkItem(px4::wq_configurations::test1) {} + ~WQueueTest() = default; + + int main(); + + void Run() override; + + static px4::AppState appState; /* track requests to terminate app */ + +private: + int _iter{0}; +}; diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 27dc1f327890..4ac2e6ebbeeb 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -100,6 +100,9 @@ typedef struct { #error "No target OS defined" #endif +// PX4 work queue starting high priority +#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 11) + // Fast drivers - they need to run as quickly as possible to minimize control // latency. #define SCHED_PRIORITY_FAST_DRIVER (SCHED_PRIORITY_MAX - 0) @@ -108,37 +111,37 @@ typedef struct { // they should be the first to run on an update, using the current sensor // data and the *previous* attitude reference from the position controller // which typically runs at a slower rate -#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 4) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (PX4_WQ_HP_BASE - 4) // Actuator outputs should run as soon as the rate controller publishes // the actuator controls topic -#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 3) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (PX4_WQ_HP_BASE - 3) // Position controllers typically are in a blocking wait on estimator data // so when new sensor data is available they will run last. Keeping them // on a high priority ensures that they are the first process to be run // when the estimator updates. -#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_POSITION_CONTROL (PX4_WQ_HP_BASE - 5) // Estimators should run after the attitude controller but before anything // else in the system. They wait on sensor data which is either coming // from the sensor hub or from a driver. Keeping this class at a higher // priority ensures that the estimator runs first if it can, but will // wait for the sensor hub if its data is coming from it. -#define SCHED_PRIORITY_ESTIMATOR (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ESTIMATOR (PX4_WQ_HP_BASE - 5) // The sensor hub conditions sensor data. It is not the fastest component // in the controller chain, but provides easy-to-use data to the more // complex downstream consumers -#define SCHED_PRIORITY_SENSOR_HUB (SCHED_PRIORITY_MAX - 6) +#define SCHED_PRIORITY_SENSOR_HUB (PX4_WQ_HP_BASE - 6) // The log capture (which stores log data into RAM) should run faster // than other components, but should not run before the control pipeline -#define SCHED_PRIORITY_LOG_CAPTURE (SCHED_PRIORITY_MAX - 10) +#define SCHED_PRIORITY_LOG_CAPTURE (PX4_WQ_HP_BASE - 10) // Slow drivers should run at a rate where they do not impact the overall // system execution -#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_SLOW_DRIVER (PX4_WQ_HP_BASE - 35) // The navigation system needs to execute regularly but has no realtime needs #define SCHED_PRIORITY_NAVIGATION (SCHED_PRIORITY_DEFAULT + 5)