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 00abbaaf05..14de3fbbe1 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 @@ -81,6 +81,8 @@ protected: explicit WorkItem(const char *name, const wq_config_t &config); + explicit WorkItem(const char *name, const WorkItem &work_item); + virtual ~WorkItem(); /** diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItemSingleShot.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItemSingleShot.hpp new file mode 100644 index 0000000000..7ff73298a5 --- /dev/null +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItemSingleShot.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 WorkItemSingleShot + * Run a method on a specific work queue and wait for it to exit. + * Example usage: + * WorkItemSingleShot initializer(px4::device_bus_to_wq(device_id.devid), instantiate, &data); + * initializer.ScheduleNow(); + * initializer.wait(); + */ +class WorkItemSingleShot : public px4::WorkItem +{ +public: + using worker_method = void (*)(void *arg); + + WorkItemSingleShot(const px4::wq_config_t &config, worker_method method, void *argument); + WorkItemSingleShot(const px4::WorkItem &work_item, worker_method method, void *argument); + + ~WorkItemSingleShot(); + + WorkItemSingleShot() = delete; + + // no copy, assignment, move, move assignment + WorkItemSingleShot(const WorkItemSingleShot &) = delete; + WorkItemSingleShot &operator=(const WorkItemSingleShot &) = delete; + WorkItemSingleShot(WorkItemSingleShot &&) = delete; + WorkItemSingleShot &operator=(WorkItemSingleShot &&) = delete; + + void wait(); +protected: + void Run() override; +private: + void *_argument; + worker_method _method; + px4_sem_t _sem; +}; + +} // namespace px4 diff --git a/platforms/common/px4_work_queue/CMakeLists.txt b/platforms/common/px4_work_queue/CMakeLists.txt index bcbf7f6bb4..a4e45a6a9a 100644 --- a/platforms/common/px4_work_queue/CMakeLists.txt +++ b/platforms/common/px4_work_queue/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_library(px4_work_queue ScheduledWorkItem.cpp WorkItem.cpp + WorkItemSingleShot.cpp WorkQueue.cpp WorkQueueManager.cpp ) diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index bccb3dd8e7..af4f3a57fd 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -50,6 +50,17 @@ WorkItem::WorkItem(const char *name, const wq_config_t &config) : } } +WorkItem::WorkItem(const char *name, const WorkItem &work_item) : + _item_name(name) +{ + px4::WorkQueue *wq = work_item._wq; + + if ((wq != nullptr) && wq->Attach(this)) { + _wq = wq; + _start_time = hrt_absolute_time(); + } +} + WorkItem::~WorkItem() { Deinit(); diff --git a/platforms/common/px4_work_queue/WorkItemSingleShot.cpp b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp new file mode 100644 index 0000000000..d31d51ab7b --- /dev/null +++ b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 + + +namespace px4 +{ + +WorkItemSingleShot::WorkItemSingleShot(const px4::wq_config_t &config, worker_method method, void *argument) + : px4::WorkItem("", config), _argument(argument), _method(method) +{ + px4_sem_init(&_sem, 0, 0); +} + +WorkItemSingleShot::WorkItemSingleShot(const px4::WorkItem &work_item, worker_method method, void *argument) + : px4::WorkItem("", work_item), _argument(argument), _method(method) +{ + px4_sem_init(&_sem, 0, 0); +} + +WorkItemSingleShot::~WorkItemSingleShot() +{ + px4_sem_destroy(&_sem); +} + +void WorkItemSingleShot::wait() +{ + while (px4_sem_wait(&_sem) != 0) {} +} + +void WorkItemSingleShot::Run() +{ + _method(_argument); + px4_sem_post(&_sem); +} + + +} // namespace px4