diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c9e66dbb12..70b09f9cfe 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -60,6 +60,8 @@ set(msg_files CollisionReport.msg ControlAllocatorStatus.msg Cpuload.msg + DatamanRequest.msg + DatamanResponse.msg DebugArray.msg DebugKeyValue.msg DebugValue.msg diff --git a/msg/DatamanRequest.msg b/msg/DatamanRequest.msg new file mode 100644 index 0000000000..9f6f3e6a78 --- /dev/null +++ b/msg/DatamanRequest.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear/lock/unlock +uint8 item # dm_item_t +uint32 index +uint8[56] data +uint32 data_length \ No newline at end of file diff --git a/msg/DatamanResponse.msg b/msg/DatamanResponse.msg new file mode 100644 index 0000000000..2a08e1d87e --- /dev/null +++ b/msg/DatamanResponse.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear/lock/unlock +uint8 item # dm_item_t +uint32 index +uint8[56] data + +uint8 STATUS_SUCCESS = 0 +uint8 STATUS_FAILURE_ID_ERR = 1 +uint8 STATUS_FAILURE_NO_DATA = 2 +uint8 STATUS_FAILURE_READ_FAILED = 3 +uint8 STATUS_FAILURE_WRITE_FAILED = 4 +uint8 STATUS_FAILURE_CLEAR_FAILED = 5 +uint8 STATUS_ALREADY_LOCKED = 6 +uint8 STATUS_ALREADY_UNLOCKED = 7 +uint8 STATUS_ITEM_LOCKED = 8 +uint8 status diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b..479b0fa141 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -45,6 +45,7 @@ add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) add_subdirectory(crypto EXCLUDE_FROM_ALL) +add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) diff --git a/src/lib/dataman_client/CMakeLists.txt b/src/lib/dataman_client/CMakeLists.txt new file mode 100644 index 0000000000..8d60c9935d --- /dev/null +++ b/src/lib/dataman_client/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2023 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(dataman_client DatamanClient.cpp) diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp new file mode 100644 index 0000000000..ffe82504e1 --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -0,0 +1,336 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 DatamanClient.cpp + */ + +#include + +DatamanClient::DatamanClient() +{ + _dataman_request_pub.advertise(); + _dataman_response_sub = orb_subscribe(ORB_ID(dataman_response)); + + if (_dataman_response_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + + } else { + // make sure we don't get any stale response by doing an orb_copy + dataman_response_s response{}; + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + _fds.fd = _dataman_response_sub; + _fds.events = POLLIN; + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .request_type = DM_GET_ID + }; + + bool success = syncHandler(request, response, timestamp, 1000_ms); + + if (success && (response.client_id > CLIENT_ID_NOT_SET)) { + + _client_id = response.client_id; + + } else { + PX4_ERR("Failed to get client ID!"); + } + } +} + +DatamanClient::~DatamanClient() +{ + if (_dataman_response_sub >= 0) { + orb_unsubscribe(_dataman_response_sub); + } +} + +bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout) +{ + bool response_received = false; + int32_t ret = 0; + hrt_abstime time_elapsed = hrt_elapsed_time(&start_time); + _dataman_request_pub.publish(request); + + while (!response_received && (time_elapsed < timeout)) { + + uint32_t timeout_ms = 1; + ret = px4_poll(&_fds, 1, timeout_ms); + + if (ret < 0) { + PX4_ERR("px4_poll returned error: %" PRIu32, ret); + break; + + } else if (ret == 0) { + + // No response received, send new request + _dataman_request_pub.publish(request); + + } else { + + bool updated = false; + orb_check(_dataman_response_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + if (response.client_id == request.client_id) { + + if ((response.request_type == request.request_type) && + (response.item == request.item) && + (response.index == request.index)) { + response_received = true; + break; + } + + } else if (request.client_id == CLIENT_ID_NOT_SET) { + + // validate timestamp from response.data + if (0 == memcmp(&(request.timestamp), &(response.data), sizeof(hrt_abstime))) { + response_received = true; + break; + } + } + } + } + + time_elapsed = hrt_elapsed_time(&start_time); + } + + if (!response_received && ret >= 0) { + PX4_ERR("timeout after %" PRIu32 " ms!", static_cast(timeout / 1000)); + } + + return response_received; +} + +bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .index = index, + .data_length = length, + .client_id = _client_id, + .request_type = DM_READ, + .item = item + }; + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("readSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + + } else { + memcpy(buffer, response.data, length); + } + } + + return success; +} + +bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .index = index, + .data_length = length, + .client_id = _client_id, + .request_type = DM_WRITE, + .item = item + }; + + memcpy(request.data, buffer, length); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("writeSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + } + } + + return success; +} + +bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout) +{ + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .client_id = _client_id, + .request_type = DM_CLEAR, + .item = item + }; + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("clearSync failed! status=%" PRIu8 ", item=%" PRIu8, + response.status, static_cast(item)); + } + } + + return success; +} + +bool DatamanClient::lockSync(dm_item_t item, hrt_abstime timeout) +{ + bool success = true; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .client_id = _client_id, + .request_type = DM_LOCK, + .item = item + }; + + dataman_response_s response{}; + response.status = dataman_response_s::STATUS_ALREADY_LOCKED; + + while (success && (response.status == dataman_response_s::STATUS_ALREADY_LOCKED)) { + success = syncHandler(request, response, timestamp, timeout); + } + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + + PX4_ERR("lockSync failed! status=%" PRIu8 ", item=%" PRIu8, + response.status, static_cast(item)); + } + } + + return success; +} + +bool DatamanClient::unlockSync(dm_item_t item, hrt_abstime timeout) +{ + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request = { + .timestamp = timestamp, + .client_id = _client_id, + .request_type = DM_UNLOCK, + .item = item + }; + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + + if (response.status == dataman_response_s::STATUS_ALREADY_UNLOCKED) { + PX4_WARN("Dataman already unlocked for item=%" PRIu8, + static_cast(item)); + + } else { + PX4_ERR("unlockSync failed! status=%" PRIu8 ", item=%" PRIu8, + response.status, static_cast(item)); + } + } + } + + return success; +} + +bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer) +{ + bool success = false; + return success; +} + +bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer) +{ + bool success = false; + return success; +} + +void DatamanClient::update() +{ +} + +bool DatamanClient::lastOperationCompleted(bool &success) +{ + return true; +} + +DatamanCache::DatamanCache(int num_items) +{ +} diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp new file mode 100644 index 0000000000..bcb303673c --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 +#include +#include +#include +#include + +using namespace time_literals; + +class DatamanClient +{ +public: + DatamanClient(); + ~DatamanClient(); + + DatamanClient(const DatamanClient &) = delete; + DatamanClient &operator=(const DatamanClient &) = delete; + + bool readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + bool writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + bool clearSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + bool lockSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + bool unlockSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + + bool readAsync(dm_item_t item, uint32_t index, uint8_t *buffer); + bool writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer); + + void update(); + + bool lastOperationCompleted(bool &success); + +private: + + enum class State { + Idle, + RequestSent + }; + + struct Request { + dm_item_t item; + unsigned index; + uint8_t *buffer; + }; + + /* Synchronous response/request handler */ + bool syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout); + + + //State _state{State::Idle}; + //Request _active_request{}; + + int32_t _dataman_response_sub{}; + uORB::Publication _dataman_request_pub{ORB_ID(dataman_request)}; + + px4_pollfd_struct_t _fds; + + uint8_t _client_id{0}; + + static constexpr uint8_t CLIENT_ID_NOT_SET{0}; +}; + + +class DatamanCache +{ +public: + DatamanCache(int num_items); + ~DatamanCache() = default; + + void resize(int num_items); + + void invalidate(); + + bool load(dm_item_t item, unsigned index); + + bool loadWait(dm_item_t item, unsigned index, uint8_t *buffer, hrt_abstime timeout); + + void update(); + + DatamanClient &client() { return _client; } + +private: + struct Item { + dataman_response_s data; + //State cache_state; + }; + Item *_items{nullptr}; + + DatamanClient _client; +}; diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index 38a5f6a36e..a15d999d38 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -52,13 +52,18 @@ #include #include +#include +#include +#include +#include + #include "dataman.h" __BEGIN_DECLS __EXPORT int dataman_main(int argc, char *argv[]); __END_DECLS -static constexpr int TASK_STACK_SIZE = 1320; +static constexpr int TASK_STACK_SIZE = 1420; /* Private File based Operations */ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count); @@ -117,74 +122,29 @@ static struct { bool silence = false; } dm_operations_data; -/** Types of function calls supported by the worker task */ -typedef enum { - dm_write_func = 0, - dm_read_func, - dm_clear_func, - dm_number_of_funcs -} dm_function_t; - -/** Work task work item */ -typedef struct { - sq_entry_t link; /**< list linkage */ - px4_sem_t wait_sem; - unsigned char first; - unsigned char func; - ssize_t result; - union { - struct { - dm_item_t item; - unsigned index; - const void *buf; - size_t count; - } write_params; - struct { - dm_item_t item; - unsigned index; - void *buf; - size_t count; - } read_params; - struct { - dm_item_t item; - } clear_params; - }; -} work_q_item_t; - -const size_t k_work_item_allocation_chunk_size = 8; - /* Usage statistics */ -static unsigned g_func_counts[dm_number_of_funcs]; - -/* table of maximum number of instances for each item type */ -static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { - DM_KEY_SAFE_POINTS_MAX, - DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_MISSION_STATE_MAX, - DM_KEY_COMPAT_MAX -}; +static unsigned g_func_counts[DM_NUMBER_OF_FUNCS]; #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ -/* Table of the len of each item type */ -static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { - sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct dataman_compat_s) + DM_SECTOR_HDR_SIZE +/* Table of the len of each item type including HDR size */ +static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = { + g_per_item_size[DM_KEY_SAFE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_1] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_MISSION_STATE] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_COMPAT] + DM_SECTOR_HDR_SIZE }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; -/* Item type lock mutexes */ -static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS]; -static px4_sem_t g_sys_state_mutex_mission; -static px4_sem_t g_sys_state_mutex_fence; +/* Item type lock for dataman client id*/ +static uint8_t g_item_locks[DM_KEY_NUM_KEYS]; + +constexpr uint8_t NOT_LOCKED = 0; // dataman client id reserved for unlock state +static uint8_t dataman_clients_count = 1; static perf_counter_t _dm_read_perf{nullptr}; static perf_counter_t _dm_write_perf{nullptr}; @@ -200,160 +160,12 @@ static enum { BACKEND_LAST } backend = BACKEND_NONE; -/* The data manager work queues */ - -typedef struct { - sq_queue_t q; /* Nuttx queue */ - px4_sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; /* Current size of queue */ - unsigned max_size; /* Maximum queue size reached */ -} work_q_t; - -static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ -static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ - -static px4_sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ static px4_sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -static void init_q(work_q_t *q) -{ - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ - px4_sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ -} - -static inline void -destroy_q(work_q_t *q) -{ - px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ -} - -static inline void -lock_queue(work_q_t *q) -{ - px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */ -} - -static inline void -unlock_queue(work_q_t *q) -{ - px4_sem_post(&(q->mutex)); /* Release the queue lock */ -} - -static work_q_item_t * -create_work_item() -{ - work_q_item_t *item; - - /* Try to reuse item from free item queue */ - lock_queue(&g_free_q); - - if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { - g_free_q.size--; - } - - unlock_queue(&g_free_q); - - /* If we there weren't any free items then obtain memory for a new ones */ - if (item == nullptr) { - item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); - - if (item) { - item->first = 1; - lock_queue(&g_free_q); - - for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { - (item + i)->first = 0; - sq_addfirst(&(item + i)->link, &(g_free_q.q)); - } - - /* Update the queue size and potentially the maximum queue size */ - g_free_q.size += k_work_item_allocation_chunk_size - 1; - - if (g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); - } - } - - /* If we got one then lock the item*/ - if (item) { - px4_sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ - - /* item->wait_sem use case is a signal */ - - px4_sem_setprotocol(&item->wait_sem, SEM_PRIO_NONE); - } - - /* return the item pointer, or nullptr if all failed */ - return item; -} - /* Work queue management functions */ -static inline void -destroy_work_item(work_q_item_t *item) -{ - px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ - /* Return the item to the free item queue for later reuse */ - lock_queue(&g_free_q); - sq_addfirst(&item->link, &(g_free_q.q)); - - /* Update the queue size and potentially the maximum queue size */ - if (++g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); -} - -static inline work_q_item_t * -dequeue_work_item() -{ - work_q_item_t *work; - - /* retrieve the 1st item on the work queue */ - lock_queue(&g_work_q); - - if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { - g_work_q.size--; - } - - unlock_queue(&g_work_q); - return work; -} - -static int -enqueue_work_item_and_wait_for_result(work_q_item_t *item) -{ - /* put the work item at the end of the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - - /* Adjust the queue size and potentially the maximum queue size */ - if (++g_work_q.size > g_work_q.max_size) { - g_work_q.max_size = g_work_q.size; - } - - unlock_queue(&g_work_q); - - /* tell the work thread that work is available */ - px4_sem_post(&g_work_queued_sema); - - /* wait for the result */ - px4_sem_wait(&item->wait_sem); - - int result = item->result; - - destroy_work_item(item); - - return result; -} - static bool is_running() { return dm_operations_data.running; @@ -375,7 +187,7 @@ calculate_offset(dm_item_t item, unsigned index) } /* Calculate and return the item index based on type and index */ - return g_key_offsets[item] + (index * g_per_item_size[item]); + return g_key_offsets[item] + (index * g_per_item_size_with_hdr[item]); } /* Each data item is stored as follows @@ -401,7 +213,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -429,7 +241,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) { - unsigned char buffer[g_per_item_size[item]]; + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ const int offset = calculate_offset(item, index); @@ -440,7 +252,7 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -511,7 +323,7 @@ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -546,7 +358,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) return -1; } - unsigned char buffer[g_per_item_size[item]]; + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -557,7 +369,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -639,7 +451,7 @@ static int _ram_clear(dm_item_t item) } buf[0] = 0; - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } return result; @@ -687,7 +499,7 @@ _file_clear(dm_item_t item) } } - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } /* Make sure data is actually written to physical media */ @@ -714,7 +526,7 @@ _file_initialize(unsigned max_offset) return -1; } - struct dataman_compat_s compat_state; + dataman_compat_s compat_state{}; dm_operations_data.silence = true; @@ -789,156 +601,6 @@ _ram_shutdown() dm_operations_data.running = false; } -/** Write to the data manager file */ -__EXPORT ssize_t -dm_write(dm_item_t item, unsigned index, const void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_write_perf); - - /* get a work item and queue up a write request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_write create_work_item failed"); - perf_end(_dm_write_perf); - return -1; - } - - work->func = dm_write_func; - work->write_params.item = item; - work->write_params.index = index; - work->write_params.buf = buf; - work->write_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_write_perf); - return ret; -} - -/** Retrieve from the data manager file */ -__EXPORT ssize_t -dm_read(dm_item_t item, unsigned index, void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_read_perf); - - /* get a work item and queue up a read request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_read create_work_item failed"); - perf_end(_dm_read_perf); - return -1; - } - - work->func = dm_read_func; - work->read_params.item = item; - work->read_params.index = index; - work->read_params.buf = buf; - work->read_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_read_perf); - return ret; -} - -/** Clear a data Item */ -__EXPORT int -dm_clear(dm_item_t item) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - /* get a work item and queue up a clear request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_clear create_work_item failed"); - return -1; - } - - work->func = dm_clear_func; - work->clear_params.item = item; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - return enqueue_work_item_and_wait_for_result(work); -} - -__EXPORT int -dm_lock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_wait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -__EXPORT int -dm_trylock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_trywait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -/** Unlock a data Item */ -__EXPORT void -dm_unlock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return; - } - - if (item >= DM_KEY_NUM_KEYS) { - return; - } - - if (g_item_locks[item]) { - px4_sem_post(g_item_locks[item]); - } -} - static int task_main(int argc, char *argv[]) { @@ -957,43 +619,32 @@ task_main(int argc, char *argv[]) return -1; } - work_q_item_t *work; - /* Initialize global variables */ g_key_offsets[0] = 0; for (int i = 0; i < ((int)DM_KEY_NUM_KEYS - 1); i++) { - g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size[i]); + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size_with_hdr[i]); } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * - g_per_item_size[DM_KEY_NUM_KEYS - 1]); + g_per_item_size_with_hdr[DM_KEY_NUM_KEYS - 1]); - for (unsigned i = 0; i < dm_number_of_funcs; i++) { + for (unsigned i = 0; i < DM_NUMBER_OF_FUNCS; i++) { g_func_counts[i] = 0; } - /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */ - px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */ - px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */ - - for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { - g_item_locks[i] = nullptr; + for (uint32_t i = 0; i < DM_KEY_NUM_KEYS; i++) { + g_item_locks[i] = false; } - g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission; - g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence; - g_task_should_exit = false; - init_q(&g_work_q); - init_q(&g_free_q); + uORB::Publication dataman_response_pub{ORB_ID(dataman_response)}; + const int dataman_request_sub = orb_subscribe(ORB_ID(dataman_request)); - px4_sem_init(&g_work_queued_sema, 1, 0); - - /* g_work_queued_sema use case is a signal */ - - px4_sem_setprotocol(&g_work_queued_sema, SEM_PRIO_NONE); + if (dataman_request_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + } _dm_read_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": read"); _dm_write_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": write"); @@ -1019,47 +670,153 @@ task_main(int argc, char *argv[]) break; } + px4_pollfd_struct_t fds; + fds.fd = dataman_request_sub; + fds.events = POLLIN; + /* Tell startup that the worker thread has completed its initialization */ px4_sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ while (true) { - /* do we need to exit ??? */ - if (!g_task_should_exit) { - /* wait for work */ - g_dm_ops->wait(&g_work_queued_sema); - } + ret = px4_poll(&fds, 1, 1000); - /* Empty the work queue */ - while ((work = dequeue_work_item())) { + if (ret > 0) { - /* handle each work item with the appropriate handler */ - switch (work->func) { - case dm_write_func: - g_func_counts[dm_write_func]++; - work->result = - g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.buf, work->write_params.count); - break; + bool updated = false; + orb_check(dataman_request_sub, &updated); - case dm_read_func: - g_func_counts[dm_read_func]++; - work->result = - g_dm_ops->read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); - break; + if (updated) { - case dm_clear_func: - g_func_counts[dm_clear_func]++; - work->result = g_dm_ops->clear(work->clear_params.item); - break; + dataman_request_s request; + orb_copy(ORB_ID(dataman_request), dataman_request_sub, &request); - default: /* should never happen */ - work->result = -1; - break; + dataman_response_s response{}; + response.client_id = request.client_id; + response.request_type = request.request_type; + response.item = request.item; + response.index = request.index; + response.status = dataman_response_s::STATUS_FAILURE_NO_DATA; + + ssize_t result; + + switch (request.request_type) { + + case DM_GET_ID: + if ((dataman_clients_count < UINT8_MAX) && (dataman_clients_count > NOT_LOCKED)) { + response.client_id = dataman_clients_count++; + /* Send the timestamp of the request over the data buffer so that the "dataman client" + * can distinguish whether the request was made by it. */ + memcpy(response.data, &request.timestamp, sizeof(hrt_abstime)); + + } else { + PX4_ERR("Max Dataman clients reached!"); + } + + break; + + case DM_WRITE: + + if ((g_item_locks[request.item] == NOT_LOCKED) || + (g_item_locks[request.item] == request.client_id)) { + + g_func_counts[DM_WRITE]++; + result = g_dm_ops->write(static_cast(request.item), request.index, + &(request.data), request.data_length); + + if (result > 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else if (result < 0) { + response.status = dataman_response_s::STATUS_FAILURE_WRITE_FAILED; + } + + } else { + response.status = dataman_response_s::STATUS_ITEM_LOCKED; + } + + break; + + case DM_READ: + + if ((g_item_locks[request.item] == NOT_LOCKED) || + (g_item_locks[request.item] == request.client_id)) { + + g_func_counts[DM_READ]++; + result = g_dm_ops->read(static_cast(request.item), request.index, + &(response.data), request.data_length); + + if (result > 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else if (result < 0) { + response.status = dataman_response_s::STATUS_FAILURE_READ_FAILED; + } + + } else { + response.status = dataman_response_s::STATUS_ITEM_LOCKED; + } + + break; + + case DM_CLEAR: + + if ((g_item_locks[request.item] == NOT_LOCKED) || + (g_item_locks[request.item] == request.client_id)) { + + g_func_counts[DM_CLEAR]++; + result = g_dm_ops->clear(static_cast(request.item)); + + if (result == 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_CLEAR_FAILED; + } + + } else { + response.status = dataman_response_s::STATUS_ITEM_LOCKED; + } + + break; + + case DM_LOCK: + + if (g_item_locks[request.item] == NOT_LOCKED || + (g_item_locks[request.item] == request.client_id)) { + g_func_counts[DM_LOCK]++; + g_item_locks[request.item] = request.client_id; + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_ALREADY_LOCKED; + } + + break; + + case DM_UNLOCK: + + if (g_item_locks[request.item] == request.client_id) { + g_func_counts[DM_UNLOCK]++; + g_item_locks[request.item] = NOT_LOCKED; + response.status = dataman_response_s::STATUS_SUCCESS; + + } else if (g_item_locks[request.item] != request.client_id) { + + response.status = dataman_response_s::STATUS_ALREADY_UNLOCKED; + + } + + break; + + default: + break; + } + + response.timestamp = hrt_absolute_time(); + dataman_response_pub.publish(response); } - - /* Inform the caller that work is done */ - px4_sem_post(&work->wait_sem); } /* time to go???? */ @@ -1068,26 +825,12 @@ task_main(int argc, char *argv[]) } } + orb_unsubscribe(dataman_request_sub); + g_dm_ops->shutdown(); - /* The work queue is now empty, empty the free queue */ - for (;;) { - if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == nullptr) { - break; - } - - if (work->first) { - free(work); - } - } - end: backend = BACKEND_NONE; - destroy_q(&g_work_q); - destroy_q(&g_free_q); - px4_sem_destroy(&g_work_queued_sema); - px4_sem_destroy(&g_sys_state_mutex_mission); - px4_sem_destroy(&g_sys_state_mutex_fence); perf_free(_dm_read_perf); _dm_read_perf = nullptr; @@ -1129,10 +872,12 @@ static void status() { /* display usage statistics */ - PX4_INFO("Writes %u", g_func_counts[dm_write_func]); - PX4_INFO("Reads %u", g_func_counts[dm_read_func]); - PX4_INFO("Clears %u", g_func_counts[dm_clear_func]); - PX4_INFO("Max Q lengths work %u, free %u", g_work_q.max_size, g_free_q.max_size); + PX4_INFO("Writes %u", g_func_counts[DM_WRITE]); + PX4_INFO("Reads %u", g_func_counts[DM_READ]); + PX4_INFO("Clears %u", g_func_counts[DM_CLEAR]); + PX4_INFO("Locks %u", g_func_counts[DM_LOCK]); + PX4_INFO("Unlocks %u", g_func_counts[DM_LOCK]); + perf_print_counter(_dm_read_perf); perf_print_counter(_dm_write_perf); } @@ -1142,7 +887,6 @@ stop() { /* Tell the worker task to shut down */ g_task_should_exit = true; - px4_sem_post(&g_work_queued_sema); } static void @@ -1276,3 +1020,11 @@ dataman_main(int argc, char *argv[]) return 0; } + +static_assert(sizeof(dataman_request_s::data) == sizeof(dataman_response_s::data), "request and response data are not the same size"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SAFE_POINT_SIZE, "mission_safe_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_FENCE_POINT_SIZE, "mission_fance_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_ITEM_SIZE, "mission_item_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SIZE, "mission_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= DATAMAN_COMPAT_SIZE, "dataman_compat_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= sizeof(hrt_abstime), "hrt_abstime can't fit in the response data"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index bcc5b2984a..9e5f719f0b 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -42,21 +42,29 @@ #include #include -#ifdef __cplusplus -extern "C" { -#endif - /** Types of items that the data manager can store */ typedef enum { - DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ - DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ - DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_SAFE_POINTS = 0, ///< Safe points coordinates, safe point 0 is home point + DM_KEY_FENCE_POINTS, ///< Fence vertex coordinates + DM_KEY_WAYPOINTS_OFFBOARD_0, ///< Mission way point coordinates sent over mavlink + DM_KEY_WAYPOINTS_OFFBOARD_1, ///< (alternate between 0 and 1) + DM_KEY_MISSION_STATE, ///< Persistent mission state DM_KEY_COMPAT, - DM_KEY_NUM_KEYS /* Total number of item types defined */ + DM_KEY_NUM_KEYS ///< Total number of item types defined } dm_item_t; +/** Types of function calls supported by the worker task */ +typedef enum { + DM_GET_ID = 0, ///< Get dataman client ID + DM_WRITE, ///< Write index for given item + DM_READ, ///< Read index for given item + DM_CLEAR, ///< Clear all index for given item + DM_LOCK, ///< Lock all items for given item types + DM_UNLOCK, ///< Unlock all items for given item types + DM_NUMBER_OF_FUNCS +} dm_function_t; + +/** The maximum number of instances for each item type */ #if defined(MEMORY_CONSTRAINED_SYSTEM) enum { DM_KEY_SAFE_POINTS_MAX = 8, @@ -67,7 +75,6 @@ enum { DM_KEY_COMPAT_MAX = 1 }; #else -/** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = 64, @@ -78,6 +85,32 @@ enum { }; #endif +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, + DM_KEY_MISSION_STATE_MAX, + DM_KEY_COMPAT_MAX +}; + +constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_safe_point_s); +constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s); +constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s); +constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s); +constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct mission_s); + +/** The table of the size of each item type */ +static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { + MISSION_SAFE_POINT_SIZE, + MISSION_FENCE_POINT_SIZE, + MISSION_ITEM_SIZE, + MISSION_ITEM_SIZE, + MISSION_SIZE, + DATAMAN_COMPAT_SIZE +}; + struct dataman_compat_s { uint64_t key; }; @@ -89,57 +122,3 @@ struct dataman_compat_s { (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \ (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \ sizeof(struct dataman_compat_s)) - -/** Retrieve from the data manager store */ -__EXPORT ssize_t -dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** write to the data manager store */ -__EXPORT ssize_t -dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ -__EXPORT int -dm_lock( - dm_item_t item /* The item type to lock */ -); - -/** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ -__EXPORT int -dm_trylock( - dm_item_t item /* The item type to lock */ -); - -/** Unlock all items of a type */ -__EXPORT void -dm_unlock( - dm_item_t item /* The item type to unlock */ -); - -/** Erase all items of this type */ -__EXPORT int -dm_clear( - dm_item_t item /* The item type to clear */ -); - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737e6e..01ccb41a77 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -131,6 +131,7 @@ px4_add_module( adsb airspeed component_general_json # for checksums.h + dataman_client drivers_accelerometer drivers_gyroscope drivers_magnetometer diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e89f0f1f42..cb1b0bc642 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -84,27 +84,28 @@ MavlinkMissionManager::init_offboard_mission() _dataman_init = true; /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE); - if (dm_lock_ret != 0) { + if (!success_lock) { PX4_ERR("DM_KEY_MISSION_STATE lock failed"); } mission_s mission_state; - int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); + if (success_lock) { + _dataman_client.unlockSync(DM_KEY_MISSION_STATE); } - if (ret > 0) { + if (success) { _dataman_id = (dm_item_t)mission_state.dataman_id; _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; _current_seq = mission_state.current_seq; - } else if (ret < 0) { - PX4_WARN("offboard mission init failed (%i)", ret); + } else { + PX4_WARN("offboard mission init failed"); } load_geofence_stats(); @@ -115,33 +116,35 @@ MavlinkMissionManager::init_offboard_mission() _my_dataman_id = _dataman_id; } -int +bool MavlinkMissionManager::load_geofence_stats() { mission_stats_entry_s stats; // initialize fence points count - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _geofence_update_counter = stats.update_counter; } - return ret; + return success; } -int +bool MavlinkMissionManager::load_safepoint_stats() { mission_stats_entry_s stats; // initialize safe points count - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; } - return ret; + return success; } /** @@ -160,20 +163,21 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun /* update mission state in dataman */ /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE); - if (dm_lock_ret != 0) { + if (!success_lock) { PX4_ERR("DM_KEY_MISSION_STATE lock failed"); } - int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); + if (success_lock) { + _dataman_client.unlockSync(DM_KEY_MISSION_STATE); } - if (res == sizeof(mission_s)) { + if (success) { /* update active mission state */ _dataman_id = dataman_id; _count[MAV_MISSION_TYPE_MISSION] = count; @@ -206,9 +210,10 @@ MavlinkMissionManager::update_geofence_count(unsigned count) stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data /* update stats in dataman */ - int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = count; } else { @@ -233,9 +238,10 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) stats.update_counter = ++_safepoint_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = count; } else { @@ -298,21 +304,20 @@ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { mission_item_s mission_item{}; - int16_t bytes_read = 0; bool read_success = false; switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: { - bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)); - read_success = (bytes_read == sizeof(mission_item_s)); + read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast(&mission_item), + sizeof(mission_item_s)); } break; case MAV_MISSION_TYPE_FENCE: { // Read a geofence point mission_fence_point_s mission_fence_point; - bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)); - read_success = (bytes_read == sizeof(mission_fence_point_s)); + read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); mission_item.nav_cmd = mission_fence_point.nav_cmd; mission_item.frame = mission_fence_point.frame; @@ -332,8 +337,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point mission_safe_point_s mission_safe_point; - bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)); - read_success = (bytes_read == sizeof(mission_safe_point_s)); + read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; mission_item.frame = mission_safe_point.frame; @@ -385,13 +390,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { mavlink_log_critical(_mavlink->get_mavlink_log_pub(), - "Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t", - (uint8_t)_mission_type, bytes_read); + "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type); /* EVENT - * @description Mission type: {1}. Number of bytes read: {2} + * @description Mission type: {1} */ - events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, - "Mission: Unable to read from storage", _mission_type, bytes_read); + events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, + "Mission: Unable to read from storage", _mission_type); } PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); @@ -961,9 +965,9 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) // switching back to idle PX4_DEBUG("locking fence dataman items"); - int ret = dm_lock(DM_KEY_FENCE_POINTS); + bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS); - if (ret == 0) { + if (success) { _geofence_locked = true; } else { @@ -1009,10 +1013,10 @@ MavlinkMissionManager::switch_to_idle_state() // when switching to idle, we *always* check if the lock was held and release it. // This is to ensure we don't end up in a state where we forget to release it. if (_geofence_locked) { - dm_unlock(DM_KEY_FENCE_POINTS); - _geofence_locked = false; - - PX4_DEBUG("unlocking geofence"); + if (_dataman_client.unlockSync(DM_KEY_FENCE_POINTS)) { + _geofence_locked = false; + PX4_DEBUG("unlocking geofence"); + } } _state = MAVLINK_WPM_STATE_IDLE; @@ -1131,7 +1135,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { dm_item_t dm_item = _transfer_dataman_id; - write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + write_failed = !_dataman_client.writeSync(dm_item, wp.seq, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); if (!write_failed) { /* waypoint marked as current */ @@ -1167,8 +1172,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.frame = mission_item.frame; if (!check_failed) { - write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } } @@ -1180,8 +1185,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_safe_point.lon = mission_item.lon; mission_safe_point.alt = mission_item.altitude; mission_safe_point.frame = mission_item.frame; - write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point, - sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_safe_point), sizeof(mission_safe_point_s)); } break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 64f6607ef3..25ef61c456 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -45,7 +45,7 @@ #pragma once -#include +#include #include #include #include @@ -95,6 +95,8 @@ private: enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) + DatamanClient _dataman_client{}; + uint64_t _time_last_recv{0}; uint64_t _time_last_sent{0}; @@ -132,7 +134,7 @@ private: static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; - bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) + bool _geofence_locked{false}; ///< if true, we currently hold the dataman lock for the geofence (transaction in progress) MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz @@ -168,10 +170,10 @@ private: int update_safepoint_count(unsigned count); /** load geofence stats from dataman */ - int load_geofence_stats(); + bool load_geofence_stats(); /** load safe point stats from dataman */ - int load_safepoint_stats(); + bool load_safepoint_stats(); /** * @brief Sends an waypoint ack message diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 30725a05d0..4532c08d17 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_module( geofence.cpp vtol_takeoff.cpp DEPENDS + dataman_client geo adsb geofence_breach_avoidance diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index 84e1991b34..cd6fe0bcbf 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -34,7 +34,6 @@ #include #include "geofence_breach_avoidance.h" #include "fake_geofence.hpp" -#include "dataman_mocks.hpp" #include using namespace matrix; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp deleted file mode 100644 index 5811863889..0000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 dataman_mocks.h - * Provides a minimal dataman implementation to compile against for testing - * - * @author Roman Bapst - * @author Julian Kent - */ -#pragma once - -#include -extern "C" { - __EXPORT ssize_t - dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** write to the data manager store */ - __EXPORT ssize_t - dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ - __EXPORT int - dm_lock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ - __EXPORT int - dm_trylock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** Unlock all items of a type */ - __EXPORT void - dm_unlock( - dm_item_t item /* The item type to unlock */ - ) {}; - - /** Erase all items of this type */ - __EXPORT int - dm_clear( - dm_item_t item /* The item type to clear */ - ) {return 0;}; -} - diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5ccf78b291..10c9bf7ddc 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,7 +43,7 @@ #include -#include +#include #include #include #include @@ -75,23 +75,26 @@ void Geofence::updateFence() { // Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer. // However this is currently not used - if (dm_lock(DM_KEY_FENCE_POINTS) != 0) { + bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS); + + if (!success) { PX4_ERR("lock failed"); return; } _updateFence(); - dm_unlock(DM_KEY_FENCE_POINTS); + _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); } void Geofence::_updateFence() { // initialize fence points count mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); int num_fence_items = 0; - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { num_fence_items = stats.num_items; _update_counter = stats.update_counter; } @@ -104,8 +107,10 @@ void Geofence::_updateFence() mission_fence_point_s mission_fence_point; bool is_circle_area = false; - if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != - sizeof(mission_fence_point_s)) { + success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, current_seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (!success) { PX4_ERR("dm_read failed"); break; } @@ -306,22 +311,25 @@ bool Geofence::isBelowMaxAltitude(float altitude) bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { - // the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means + // the following uses readSync, so first we try to lock all items. If that fails, it (most likely) means // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now - if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) { + bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS); + + if (!success) { return true; } // we got the lock, now check if the fence data got updated mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) { + if (success && _update_counter != stats.update_counter) { _updateFence(); } if (isEmpty()) { - dm_unlock(DM_KEY_FENCE_POINTS); + _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); /* Empty fence -> accept all points */ return true; } @@ -329,7 +337,7 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) /* Vertical check */ if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly if (altitude > _altitude_max || altitude < _altitude_min) { - dm_unlock(DM_KEY_FENCE_POINTS); + _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); return false; } } @@ -375,7 +383,7 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) } } - dm_unlock(DM_KEY_FENCE_POINTS); + _dataman_client.unlockSync(DM_KEY_FENCE_POINTS); return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; } @@ -394,13 +402,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, bool c = false; for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, + reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); + + if (!success) { break; } - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, + reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); + + if (!success) { break; } @@ -426,9 +439,10 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, { mission_fence_point_s circle_point{}; + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, polygon.dataman_index, + reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + if (!success) { PX4_ERR("dm_read failed"); return false; } @@ -531,7 +545,10 @@ Geofence::loadFromFile(const char *filename) } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, &vertex, sizeof(vertex)) != sizeof(vertex)) { + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast(&vertex), + sizeof(vertex)); + + if (!success) { goto error; } @@ -555,22 +572,31 @@ Geofence::loadFromFile(const char *filename) if (gotVertical && pointCounter > 2) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t"); events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported"); - ret_val = PX4_OK; + ret_val = PX4_ERROR; /* do a second pass, now that we know the number of vertices */ for (int seq = 1; seq <= pointCounter; ++seq) { mission_fence_point_s mission_fence_point; - if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == - sizeof(mission_fence_point_s)) { + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (success) { mission_fence_point.vertex_count = pointCounter; - dm_write(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)); + _dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); } } mission_stats_entry_s stats; stats.num_items = pointCounter; - ret_val = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (success) { + ret_val = PX4_OK; + } } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); @@ -586,7 +612,7 @@ error: int Geofence::clearDm() { - dm_clear(DM_KEY_FENCE_POINTS); + _dataman_client.clearSync(DM_KEY_FENCE_POINTS); updateFence(); return PX4_OK; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index b7386db830..ffcb800cb2 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,7 @@ #include +#include #include #include #include @@ -166,6 +167,8 @@ private: Navigator *_navigator{nullptr}; PolygonInfo *_polygons{nullptr}; + DatamanClient _dataman_client{}; + hrt_abstime _last_horizontal_range_warning{0}; hrt_abstime _last_vertical_range_warning{0}; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index eade1446bf..8128380973 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include #include @@ -75,7 +75,7 @@ void Mission::mission_init() // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start mission_s mission{}; - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { if ((mission.timestamp != 0) && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { if (mission.count > 0) { @@ -88,7 +88,7 @@ void Mission::mission_init() // initialize mission state in dataman mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.timestamp = hrt_absolute_time(); - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); } } } @@ -124,14 +124,11 @@ Mission::on_inactive() /* load missions from storage */ mission_s mission_state = {}; - dm_lock(DM_KEY_MISSION_STATE); - /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - dm_unlock(DM_KEY_MISSION_STATE); - - if (read_res == sizeof(mission_s)) { + if (success) { _mission.dataman_id = mission_state.dataman_id; _mission.count = mission_state.count; _current_mission_index = mission_state.current_seq; @@ -464,10 +461,12 @@ Mission::find_mission_land_start() bool found_land_start_marker = false; for (size_t i = 1; i < _mission.count; i++) { - const ssize_t len = sizeof(missionitem); missionitem_prev = missionitem; // store the last mission item before reading a new one - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; @@ -668,9 +667,11 @@ Mission::advance_mission() for (int32_t i = _current_mission_index - 1; i >= 0; i--) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; @@ -1641,13 +1642,14 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) return false; } - const ssize_t len = sizeof(struct mission_item_s); - /* read mission item to temp storage first to not overwrite current mission item if data damaged */ struct mission_item_s mission_item_tmp; /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { + bool success = _dataman_client.readSync(dm_item, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, @@ -1668,7 +1670,10 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { + success = _dataman_client.writeSync(dm_item, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + sizeof(struct mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the dataman */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, @@ -1718,16 +1723,17 @@ Mission::save_mission_state() mission_s mission_state = {}; /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE); - if (dm_lock_ret != 0) { + if (!success_lock) { PX4_ERR("DM_KEY_MISSION_STATE lock failed"); } /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - if (read_res == sizeof(mission_s)) { + if (success) { /* data read successfully, check dataman ID and items count */ if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { /* navigator may modify only sequence, write modified state only if it changed */ @@ -1735,7 +1741,10 @@ Mission::save_mission_state() mission_state.current_seq = _current_mission_index; mission_state.timestamp = hrt_absolute_time(); - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } @@ -1756,15 +1765,18 @@ Mission::save_mission_state() events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state"); /* write modified state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } } /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); + if (success_lock) { + _dataman_client.unlockSync(DM_KEY_MISSION_STATE); } } @@ -1821,9 +1833,9 @@ Mission::check_mission_valid(bool force) void Mission::reset_mission(struct mission_s &mission) { - dm_lock(DM_KEY_MISSION_STATE); + _dataman_client.lockSync(DM_KEY_MISSION_STATE); - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { /* set current item to 0 */ mission.current_seq = 0; @@ -1836,7 +1848,9 @@ Mission::reset_mission(struct mission_s &mission) struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); - if (dm_read(dm_current, index, &item, len) != len) { + bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast(&item), sizeof(mission_item_s)); + + if (!success) { PX4_WARN("could not read mission item during reset"); break; } @@ -1844,7 +1858,9 @@ Mission::reset_mission(struct mission_s &mission) if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; - if (dm_write(dm_current, index, &item, len) != len) { + success = _dataman_client.writeSync(dm_current, index, reinterpret_cast(&item), len); + + if (!success) { PX4_WARN("could not save mission item during reset"); break; } @@ -1863,10 +1879,10 @@ Mission::reset_mission(struct mission_s &mission) mission.current_seq = 0; } - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); } - dm_unlock(DM_KEY_MISSION_STATE); + _dataman_client.unlockSync(DM_KEY_MISSION_STATE); } bool @@ -1883,7 +1899,7 @@ Mission::need_to_reset_mission() } int32_t -Mission::index_closest_mission_item() const +Mission::index_closest_mission_item() { int32_t min_dist_index(0); float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); @@ -1892,9 +1908,11 @@ Mission::index_closest_mission_item() const for (size_t i = 0; i < _mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4854ee0f05..6cef70c4e7 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -52,7 +52,7 @@ #include -#include +#include #include #include #include @@ -234,7 +234,7 @@ private: /** * Return the index of the closest mission item to the current global position. */ - int32_t index_closest_mission_item() const; + int32_t index_closest_mission_item(); bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; @@ -323,6 +323,7 @@ private: uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ mission_s _mission {}; + DatamanClient _dataman_client{}; int32_t _current_mission_index{-1}; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ba21850c6c..afe8f3ba00 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -78,9 +78,11 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { _navigator->get_mission_result()->warning = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; @@ -116,9 +118,11 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al if (_navigator->get_geofence().valid()) { for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 902d803eda..f992b2e6a3 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include "MissionFeasibility/FeasibilityChecker.hpp" @@ -75,4 +75,6 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(const mission_s &mission); + + DatamanClient _dataman_client{}; }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 66e4f83a1e..12a096ec87 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e17634ada5..dd4d863716 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,7 +42,7 @@ #include "rtl.h" #include "navigator.h" -#include +#include #include #include @@ -181,10 +181,11 @@ void RTL::find_RTL_destination() // compare to safe landing positions mission_safe_point_s closest_safe_point {}; mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); int num_safe_points = 0; - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { num_safe_points = stats.num_items; } @@ -194,8 +195,10 @@ void RTL::find_RTL_destination() for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { mission_safe_point_s mission_safe_point; - if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != - sizeof(mission_safe_point_s)) { + success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, current_seq, reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); + + if (!success) { PX4_ERR("dm_read failed"); continue; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3c6dea1a2b..6b8ddd28d2 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -53,6 +53,7 @@ #include #include #include +#include class Navigator; @@ -156,6 +157,8 @@ private: } }; + DatamanClient _dataman_client{}; + RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) hrt_abstime _destination_check_time{0}; diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index ab948cf8a2..72d99c6708 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -36,7 +36,7 @@ set(srcs test_bezierQuad.cpp test_bitset.cpp test_bson.cpp - test_dataman.c + test_dataman.cpp test_file.c test_file2.c test_float.cpp @@ -93,6 +93,7 @@ px4_add_module( SRCS ${srcs} DEPENDS + dataman_client version ) diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c deleted file mode 100644 index e553323060..0000000000 --- a/src/systemcmds/tests/test_dataman.c +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-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 test_dataman.c - * Tests for the data manager. - */ - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "tests_main.h" - -#include "dataman/dataman.h" - -static px4_sem_t *sems; -static bool *task_returned_error; -int test_dataman(int argc, char *argv[]); - -#define NUM_MISSIONS_TEST 50 - -#define DM_MAX_DATA_SIZE sizeof(struct mission_s) - -static int -task_main(int argc, char *argv[]) -{ - char buffer[DM_MAX_DATA_SIZE]; - - PX4_INFO("Starting dataman test task %s", argv[2]); - /* try to read an invalid item */ - int my_id = atoi(argv[2]); - - /* try to read an invalid item */ - if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid item failed", my_id); - goto fail; - } - - /* try to read an invalid index */ - if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid index failed", my_id); - goto fail; - } - - srand(hrt_absolute_time() ^ my_id); - unsigned hit = 0; - unsigned miss = 0; - hrt_abstime wstart = hrt_absolute_time(); - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - memset(buffer, my_id, sizeof(buffer)); - buffer[1] = i; - unsigned hash = i ^ my_id; - unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, len); - //PX4_INFO("ret: %d", ret); - - if (ret != len) { - PX4_WARN("task %d: write failed ret=%d, index: %d, length: %d", my_id, ret, hash, len); - goto fail; - } - - if (i % (NUM_MISSIONS_TEST / 10) == 0) { - PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST); - } - - px4_usleep(rand() & ((64 * 1024) - 1)); - } - - hrt_abstime rstart = hrt_absolute_time(); - hrt_abstime wend = rstart; - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - unsigned hash = i ^ my_id; - ssize_t len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer)); - ssize_t len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, ret=%zd, len=%zd", my_id, hash, len2, len); - goto fail; - } - - if (buffer[0] == my_id) { - hit++; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, wanted %zd, got %zd", my_id, hash, len, len2); - goto fail; - } - - if (buffer[1] != i) { - PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); - goto fail; - } - - } else { - miss++; - } - } - - hrt_abstime rend = hrt_absolute_time(); - PX4_INFO("task %d pass, hit %d, miss %d, io time read %" PRIu64 "ms. write %" PRIu64 "ms.", - my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000); - px4_sem_post(sems + my_id); - return 0; - -fail: - PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x", - my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); - px4_sem_post(sems + my_id); - task_returned_error[my_id] = true; - return -1; -} - -int test_dataman(int argc, char *argv[]) -{ - int i = 0; - unsigned num_tasks = 4; - char buffer[DM_MAX_DATA_SIZE]; - - if (argc > 1) { - num_tasks = atoi(argv[1]); - } - - sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t)); - task_returned_error = (bool *)calloc(num_tasks, sizeof(bool)); - PX4_INFO("Running %d tasks", num_tasks); - - for (i = 0; i < num_tasks; i++) { - int task; - - char a[16]; - snprintf(a, 16, "%d", i); - - char *av[] = {"tests_dataman", a, NULL}; - - px4_sem_init(sems + i, 1, 0); - /* sems use case is a signal */ - px4_sem_setprotocol(sems + i, SEM_PRIO_NONE); - - /* start the task */ - if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, task_main, av)) <= 0) { - PX4_ERR("task start failed"); - } - } - - for (i = 0; i < num_tasks; i++) { - px4_sem_wait(sems + i); - px4_sem_destroy(sems + i); - } - - free(sems); - - bool got_error = false; - - for (i = 0; i < num_tasks; i++) { - if (task_returned_error[i]) { - got_error = true; - break; - } - } - - free(task_returned_error); - - if (got_error) { - return -1; - } - - for (i = 0; i < NUM_MISSIONS_TEST; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { - break; - } - } - - return 0; -} diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp new file mode 100644 index 0000000000..745ab69ff6 --- /dev/null +++ b/src/systemcmds/tests/test_dataman.cpp @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 test_dataman.cpp + * Tests for Dataman. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dataman_client/DatamanClient.hpp" + +class DatamanTest : public UnitTest +{ +public: + virtual bool run_tests(); + +private: + bool testSyncReadInvalidItem(); + bool testSyncWriteInvalidItem(); + + bool testSyncReadInvalidIndex(); + bool testSyncWriteInvalidIndex(); + + bool testSyncReadBufferOverflow(); + bool testSyncWriteBufferOverflow(); + + bool testMutipleClients(); + + bool testSyncWriteReadAllItemsMaxSize(); + + bool testSyncClearAll(); + + bool testResetItems(); + + static void *testLockThread(void *arg); + + DatamanClient _dataman_client1{}; + DatamanClient _dataman_client2{}; + DatamanClient _dataman_client3{}; + DatamanClient _dataman_client_thread1{}; + + static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_ITEM_SIZE}; + static_assert(sizeof(dataman_response_s::data) == DM_MAX_DATA_SIZE, "data size != DM_MAX_DATA_SIZE"); + + uint8_t _buffer_read[DM_MAX_DATA_SIZE]; + uint8_t _buffer_write[DM_MAX_DATA_SIZE]; + + static constexpr uint32_t OVERFLOW_LENGTH = sizeof(_buffer_write) + 1; +}; + +bool +DatamanTest::testSyncReadInvalidItem() +{ + + bool success = _dataman_client1.readSync(DM_KEY_NUM_KEYS, 0, _buffer_read, sizeof(_buffer_read)); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidItem() +{ + bool success = _dataman_client1.writeSync(DM_KEY_NUM_KEYS, 0, _buffer_write, sizeof(_buffer_write)); + return !success; +} + + +bool +DatamanTest::testSyncReadInvalidIndex() +{ + bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 0); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidIndex() +{ + bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 0); + return !success; +} + +bool +DatamanTest::testSyncReadBufferOverflow() +{ + bool success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_read, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testSyncWriteBufferOverflow() +{ + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_write, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testMutipleClients() +{ + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client2.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client3.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + //Test locking + success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE); + + if (!success) { + return false; + } + + //Check if already locked + success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE, 10_ms); + + if (success) { + return false; + } + + //Check if already locked from another client + success = _dataman_client2.lockSync(DM_KEY_MISSION_STATE, 10_ms); + + if (success) { + return false; + } + + //Check if can write while locked + success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, _buffer_write, g_per_item_size[DM_KEY_MISSION_STATE]); + + if (!success) { + return false; + } + + //Check if can read while locked + success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, _buffer_read, g_per_item_size[DM_KEY_MISSION_STATE]); + + if (!success) { + return false; + } + + //Compare content from buffers + for (uint32_t i = 0; i < g_per_item_size[DM_KEY_MISSION_STATE]; ++i) { + if (_buffer_read[i] != _buffer_write[i]) { + return false; + } + } + + //Check if can write while locked from another client + success = _dataman_client2.writeSync(DM_KEY_MISSION_STATE, 0, _buffer_write, g_per_item_size[DM_KEY_MISSION_STATE]); + + if (success) { + return false; + } + + //Check if can read while locked from another client + success = _dataman_client2.readSync(DM_KEY_MISSION_STATE, 0, _buffer_read, g_per_item_size[DM_KEY_MISSION_STATE]); + + if (success) { + return false; + } + + //Check if can unlock while locked from another client + success = _dataman_client2.unlockSync(DM_KEY_MISSION_STATE); + + if (success) { + return false; + } + + //Check if can unlock + success = _dataman_client1.unlockSync(DM_KEY_MISSION_STATE); + + if (!success) { + return false; + } + + //Lock from another thread and to test retry lock mechanism + pthread_t thread{}; + uint32_t ret = pthread_create(&thread, NULL, &testLockThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + return false; + } + + px4_usleep(50_ms); + + //Should fail since timeout is to short + success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE, 10_ms); + + if (success) { + return false; + } + + //Should be able to lock since the task in the thread should unlock the item in the meantime + success = _dataman_client1.lockSync(DM_KEY_MISSION_STATE); + + if (!success) { + return false; + } + + success = _dataman_client1.unlockSync(DM_KEY_MISSION_STATE); + + if (!success) { + return false; + } + + pthread_join(thread, nullptr); + + return success; +} + +void *DatamanTest::testLockThread(void *arg) +{ + DatamanTest *dataman_test = (DatamanTest *)arg; + dataman_test->_dataman_client_thread1.lockSync(DM_KEY_MISSION_STATE); + px4_usleep(200_ms); + dataman_test->_dataman_client_thread1.unlockSync(DM_KEY_MISSION_STATE); + px4_usleep(200_ms); + + return nullptr; +} + +bool +DatamanTest::testSyncWriteReadAllItemsMaxSize() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + // writeSync + for (uint32_t index = 0U; index < g_per_item_max_index[item]; ++index) { + + // Prepare write buffer + for (uint32_t i = 0; i < g_per_item_size[item]; ++i) { + _buffer_write[i] = (uint8_t)(index % UINT8_MAX); + } + + success = _dataman_client1.writeSync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]); + + if (!success) { + PX4_ERR("writeSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + } + + // readSync + for (volatile uint32_t index = 0U; index < g_per_item_max_index[item]; ++index) { + + success = _dataman_client1.readSync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]); + + if (!success) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + + // Check read buffer + for (volatile uint32_t i = 0U; i < g_per_item_size[item]; ++i) { + + uint8_t expected_value = (index % UINT8_MAX); + + if (expected_value != _buffer_read[i]) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32 ", element= %" PRIu32 ", expected: %" PRIu8 + ", received: %" PRIu8, + item, index, i, expected_value, _buffer_read[i]); + return false; + } + } + } + } + + return true;; +} + +bool +DatamanTest::testSyncClearAll() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + success = _dataman_client1.clearSync((dm_item_t)item); + + if (!success) { + PX4_ERR("clearSync failed at item = %" PRIu32, item); + return false; + } + } + + return success; +} + +bool +DatamanTest::testResetItems() +{ + bool success = false; + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; + + success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_MISSION_STATE"); + return false; + } + + success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to read DM_KEY_MISSION_STATE"); + return false; + } + + mission_stats_entry_s stats; + stats.num_items = 0; + stats.update_counter = 1; + + success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_FENCE_POINTS"); + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_SAFE_POINTS"); + return false; + } + + return success; +} + +bool DatamanTest::run_tests() +{ + ut_run_test(testSyncReadInvalidItem); + ut_run_test(testSyncWriteInvalidItem); + + ut_run_test(testSyncReadInvalidIndex); + ut_run_test(testSyncWriteInvalidIndex); + + ut_run_test(testSyncReadBufferOverflow); + ut_run_test(testSyncWriteBufferOverflow); + + ut_run_test(testMutipleClients); + + ut_run_test(testSyncWriteReadAllItemsMaxSize); + + ut_run_test(testSyncClearAll); + + ut_run_test(testResetItems); + + return (_tests_failed == 0); +} + +ut_declare_test_c(test_dataman, DatamanTest)