fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 14:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

dataman: add DatamanClient with sync functions

Browse Source 
Rework of dataman
This commit is contained in:
Igor Mišić
2023-07-24 13:10:31 +02:00
committed by Beat Küng
parent c67af1479f
commit 208552fdab
27 changed files with 1392 additions and 948 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/dataman/dataman.cpp
+192 -440
View File
@@ -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 <lib/perf/perf_counter.h>
#include <stdlib.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/dataman_request.h>
#include <uORB/topics/dataman_response.h>
#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_s> 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<dm_item_t>(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<dm_item_t>(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<dm_item_t>(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");
src/modules/dataman/dataman.h
+45 -66
View File
@@ -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 <navigator/navigation.h>
#include <uORB/topics/mission.h>
#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
src/modules/mavlink/CMakeLists.txt
+1
View File
@@ -131,6 +131,7 @@ px4_add_module(
adsb
airspeed
component_general_json # for checksums.h
dataman_client
drivers_accelerometer
drivers_gyroscope
drivers_magnetometer
src/modules/mavlink/mavlink_mission.cpp
+54 -49
View File
@@ -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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t, int16_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
"Mission: Unable to read from storage", _mission_type, bytes_read);
events::send<uint8_t>(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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&mission_safe_point), sizeof(mission_safe_point_s));
}
break;
src/modules/mavlink/mavlink_mission.h
+6 -4
View File
@@ -45,7 +45,7 @@
#pragma once
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/mission_result.h>
@@ -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
src/modules/navigator/CMakeLists.txt
+1
View File
@@ -51,6 +51,7 @@ px4_add_module(
geofence.cpp
vtol_takeoff.cpp
DEPENDS
dataman_client
geo
adsb
geofence_breach_avoidance
src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
-1
View File
@@ -34,7 +34,6 @@
#include <gtest/gtest.h>
#include "geofence_breach_avoidance.h"
#include "fake_geofence.hpp"
#include "dataman_mocks.hpp"
#include <parameters/param.h>
using namespace matrix;
src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp
-93
View File
@@ -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 <dataman/dataman.h>
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;};
}
src/modules/navigator/geofence.cpp
+53 -27
View File
@@ -43,7 +43,7 @@
#include <ctype.h>
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <systemlib/mavlink_log.h>
@@ -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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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;
}
src/modules/navigator/geofence.h
+3
View File
@@ -42,6 +42,7 @@
#include <float.h>
#include <dataman_client/DatamanClient.hpp>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
@@ -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};
src/modules/navigator/mission.cpp
+52 -34
View File
@@ -50,7 +50,7 @@
#include <string.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <lib/geo/geo.h>
@@ -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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint16_t>(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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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;
src/modules/navigator/mission.h
+3 -2
View File
@@ -52,7 +52,7 @@
#include <float.h>
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
@@ -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};
src/modules/navigator/mission_feasibility_checker.cpp
+8 -4
View File
@@ -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<uint8_t *>(&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<uint8_t *>(&missionitem),
sizeof(mission_item_s));
if (!success) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
src/modules/navigator/mission_feasibility_checker.h
+3 -1
View File
@@ -42,7 +42,7 @@
#pragma once
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <uORB/topics/mission.h>
#include <px4_platform_common/module_params.h>
#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{};
};
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -48,7 +48,7 @@
#include <float.h>
#include <sys/stat.h>
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/adsb/AdsbConflict.h>
src/modules/navigator/rtl.cpp
+8 -5
View File
@@ -42,7 +42,7 @@
#include "rtl.h"
#include "navigator.h"
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
@@ -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<uint8_t *>(&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<uint8_t *>(&mission_safe_point),
sizeof(mission_safe_point_s));
if (!success) {
PX4_ERR("dm_read failed");
continue;
}
src/modules/navigator/rtl.h
+3
View File
@@ -53,6 +53,7 @@
#include <uORB/topics/wind.h>
#include <matrix/math.hpp>
#include <lib/geo/geo.h>
#include <dataman_client/DatamanClient.hpp>
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};