mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 00:03:33 +08:00
Add data manager module and fence support to navigator
- Add function to geo.c to determine if global position is inside fence - Add navigator support/commands for maintaining fence coords. - Add data manager module to support persistence fence storage. Can store other data, but only used for fence at this time. - Add unit tests for data manager
This commit is contained in:
Jean Cyr
@@ -0,0 +1,739 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier
|
||||
* Jean Cyr
|
||||
*
|
||||
* 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 navigator_main.c
|
||||
* Implementation of the main navigation state machine.
|
||||
*
|
||||
* Handles missions, geo fencing and failsafe navigation behavior.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <queue.h>
|
||||
|
||||
#include "dataman.h"
|
||||
|
||||
/**
|
||||
* data manager app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
|
||||
__EXPORT int dataman_main(int argc, char *argv[]);
|
||||
__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
|
||||
__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
|
||||
__EXPORT int dm_clear(dm_item_t item);
|
||||
__EXPORT int dm_restart(dm_reset_reason restart_type);
|
||||
|
||||
/* Types of function calls supported by the worker task */
|
||||
typedef enum {
|
||||
dm_write_func = 0,
|
||||
dm_read_func,
|
||||
dm_clear_func,
|
||||
dm_restart_func,
|
||||
dm_number_of_funcs
|
||||
} dm_function_t;
|
||||
|
||||
/* Work task work item */
|
||||
typedef struct {
|
||||
sq_entry_t link; /**< list linkage */
|
||||
sem_t wait_sem;
|
||||
dm_function_t func;
|
||||
ssize_t result;
|
||||
union {
|
||||
struct {
|
||||
dm_item_t item;
|
||||
unsigned char index;
|
||||
dm_persitence_t persistence;
|
||||
const void *buf;
|
||||
size_t count;
|
||||
} write_params;
|
||||
struct {
|
||||
dm_item_t item;
|
||||
unsigned char index;
|
||||
void *buf;
|
||||
size_t count;
|
||||
} read_params;
|
||||
struct {
|
||||
dm_item_t item;
|
||||
} clear_params;
|
||||
struct {
|
||||
dm_reset_reason reason;
|
||||
} restart_params;
|
||||
};
|
||||
} work_q_item_t;
|
||||
|
||||
/* 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_WAY_POINTS_MAX,
|
||||
};
|
||||
|
||||
/* Table of offset for index 0 of each item type */
|
||||
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
|
||||
|
||||
/* The data manager store file handle and file name */
|
||||
static int g_fd = -1, g_task_fd = -1;
|
||||
static const char *k_data_manager_device_path = "/fs/microsd/dataman";
|
||||
|
||||
/* The data manager work queues */
|
||||
|
||||
typedef struct {
|
||||
sq_queue_t q;
|
||||
sem_t mutex; /* Mutual exclusion on work queue adds and deletes */
|
||||
unsigned size;
|
||||
unsigned max_size;
|
||||
} work_q_t;
|
||||
|
||||
static work_q_t g_free_q;
|
||||
static work_q_t g_work_q;
|
||||
|
||||
sem_t g_work_queued_sema;
|
||||
sem_t g_init_sema;
|
||||
|
||||
static bool g_task_should_exit; /**< if true, sensor task should exit */
|
||||
|
||||
#define DM_SECTOR_HDR_SIZE 4
|
||||
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
|
||||
|
||||
static void init_q(work_q_t *q)
|
||||
{
|
||||
sq_init(&(q->q));
|
||||
sem_init(&(q->mutex), 1, 1);
|
||||
q->size = q->max_size = 0;
|
||||
}
|
||||
|
||||
static void destroy_q(work_q_t *q)
|
||||
{
|
||||
sem_destroy(&(q->mutex));
|
||||
}
|
||||
|
||||
static inline void
|
||||
lock_queue(work_q_t *q)
|
||||
{
|
||||
sem_wait(&(q->mutex));
|
||||
}
|
||||
|
||||
static inline void
|
||||
unlock_queue(work_q_t *q)
|
||||
{
|
||||
sem_post(&(q->mutex));
|
||||
}
|
||||
|
||||
static work_q_item_t *
|
||||
create_work_item(void)
|
||||
{
|
||||
work_q_item_t *item;
|
||||
|
||||
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 (item == NULL)
|
||||
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
|
||||
|
||||
if (item)
|
||||
sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
|
||||
|
||||
return item;
|
||||
}
|
||||
|
||||
/* Work queue management functions */
|
||||
static void
|
||||
enqueue_work_item(work_q_item_t *item)
|
||||
{
|
||||
/* put the work item on the work queue */
|
||||
lock_queue(&g_work_q);
|
||||
sq_addlast(&item->link, &(g_work_q.q));
|
||||
|
||||
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 */
|
||||
sem_post(&g_work_queued_sema);
|
||||
}
|
||||
|
||||
static void
|
||||
destroy_work_item(work_q_item_t *item)
|
||||
{
|
||||
sem_destroy(&item->wait_sem);
|
||||
lock_queue(&g_free_q);
|
||||
sq_addfirst(&item->link, &(g_free_q.q));
|
||||
|
||||
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 work_q_item_t *
|
||||
dequeue_work_item(void)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
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;
|
||||
}
|
||||
|
||||
/* Calculate the offset in file of specific item */
|
||||
static int
|
||||
calculate_offset(dm_item_t item, unsigned char index)
|
||||
{
|
||||
|
||||
/* Make sure the item type is valid */
|
||||
if (item >= DM_KEY_NUM_KEYS)
|
||||
return -1;
|
||||
|
||||
/* Make sure the index for this item type is valid */
|
||||
if (index >= g_per_item_max_index[item])
|
||||
return -1;
|
||||
|
||||
/* Calculate and return the item index based on type and index */
|
||||
return g_key_offsets[item] + (index * k_sector_size);
|
||||
}
|
||||
|
||||
/* Each data item is stored as follows
|
||||
*
|
||||
* byte 0: Length of user data item
|
||||
* byte 1: Persistence of this data item
|
||||
* byte DM_SECTOR_HDR_SIZE... : data item value
|
||||
*
|
||||
* The total size must not exceed k_sector_size
|
||||
*/
|
||||
|
||||
/* write to the data manager file */
|
||||
static ssize_t
|
||||
_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
|
||||
{
|
||||
unsigned char buffer[k_sector_size];
|
||||
size_t len;
|
||||
int offset;
|
||||
|
||||
/* Get the offset for this item */
|
||||
offset = calculate_offset(item, index);
|
||||
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
/* Make sure caller has not given us more data than we can handle */
|
||||
if (count > DM_MAX_DATA_SIZE)
|
||||
return -1;
|
||||
|
||||
/* Write out the data, prefixed with length and persistence level */
|
||||
buffer[0] = count;
|
||||
buffer[1] = persistence;
|
||||
buffer[2] = 0;
|
||||
buffer[3] = 0;
|
||||
memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
|
||||
count += DM_SECTOR_HDR_SIZE;
|
||||
|
||||
len = -1;
|
||||
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
|
||||
if ((len = write(g_task_fd, buffer, count)) == count)
|
||||
fsync(g_task_fd);
|
||||
|
||||
if (len != count)
|
||||
return -1;
|
||||
|
||||
/* All is well... return the number of user data written */
|
||||
return count - DM_SECTOR_HDR_SIZE;
|
||||
}
|
||||
|
||||
/* Retrieve from the data manager file */
|
||||
static ssize_t
|
||||
_read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
||||
{
|
||||
unsigned char buffer[k_sector_size];
|
||||
int len, offset;
|
||||
|
||||
/* Get the offset for this item */
|
||||
offset = calculate_offset(item, index);
|
||||
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
/* Make sure the caller hasn't asked for more data than we can handle */
|
||||
if (count > DM_MAX_DATA_SIZE)
|
||||
return -1;
|
||||
|
||||
/* Read the prefix and data */
|
||||
len = -1;
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
|
||||
len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
|
||||
|
||||
/* Check for length issues */
|
||||
if (len < 0)
|
||||
return -1;
|
||||
|
||||
if (len == 0)
|
||||
buffer[0] = 0;
|
||||
|
||||
if (buffer[0] > 0) {
|
||||
if (buffer[0] > count)
|
||||
return -1;
|
||||
|
||||
/* Looks good, copy it to the caller's buffer */
|
||||
memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
|
||||
}
|
||||
|
||||
/* Return the number of bytes of caller data read */
|
||||
return buffer[0];
|
||||
}
|
||||
|
||||
static int
|
||||
_clear(dm_item_t item)
|
||||
{
|
||||
int i, result = 0;
|
||||
|
||||
int offset = calculate_offset(item, 0);
|
||||
|
||||
if (offset < 0)
|
||||
return -1;
|
||||
|
||||
for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
|
||||
char buf[1];
|
||||
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
if (read(g_task_fd, buf, 1) < 1)
|
||||
break;
|
||||
|
||||
if (buf[0]) {
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
buf[0] = 0;
|
||||
|
||||
if (write(g_task_fd, buf, 1) != 1) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
offset += k_sector_size;
|
||||
}
|
||||
|
||||
fsync(g_task_fd);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Tell the data manager about the type of the last reset */
|
||||
static int
|
||||
_restart(dm_reset_reason reason)
|
||||
{
|
||||
unsigned char buffer[2];
|
||||
int offset, result = 0;
|
||||
|
||||
/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
|
||||
|
||||
/* Loop through all of the data segments and delete those that are not persistent */
|
||||
offset = 0;
|
||||
|
||||
while (1) {
|
||||
size_t len;
|
||||
|
||||
/* Get data segment at current offset */
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
len = read(g_task_fd, buffer, sizeof(buffer));
|
||||
|
||||
if (len == 0)
|
||||
break;
|
||||
|
||||
/* check if segment contains data */
|
||||
if (buffer[0]) {
|
||||
int clear_entry = 0;
|
||||
|
||||
/* Whether data gets deleted depends on reset type and data segment's persistence setting */
|
||||
if (reason == DM_INIT_REASON_POWER_ON) {
|
||||
if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
|
||||
clear_entry = 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
|
||||
clear_entry = 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* Set segment to unused if data does not persist */
|
||||
if (clear_entry) {
|
||||
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
buffer[0] = 0;
|
||||
|
||||
len = write(g_task_fd, buffer, 1);
|
||||
|
||||
if (len != 1) {
|
||||
result = -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
offset += k_sector_size;
|
||||
}
|
||||
|
||||
fsync(g_task_fd);
|
||||
|
||||
/* tell the caller how it went */
|
||||
return result;
|
||||
}
|
||||
|
||||
/* write to the data manager file */
|
||||
__EXPORT ssize_t
|
||||
dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
|
||||
work->func = dm_write_func;
|
||||
work->write_params.item = item;
|
||||
work->write_params.index = index;
|
||||
work->write_params.persistence = persistence;
|
||||
work->write_params.buf = buf;
|
||||
work->write_params.count = count;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
ssize_t result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Retrieve from the data manager file */
|
||||
__EXPORT ssize_t
|
||||
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
|
||||
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_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
ssize_t result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
}
|
||||
|
||||
__EXPORT int
|
||||
dm_clear(dm_item_t item)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
|
||||
work->func = dm_clear_func;
|
||||
work->clear_params.item = item;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
int result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Tell the data manager about the type of the last reset */
|
||||
__EXPORT int
|
||||
dm_restart(dm_reset_reason reason)
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
if ((g_fd < 0) || g_task_should_exit)
|
||||
return -1;
|
||||
|
||||
/* Will return with queues locked */
|
||||
if ((work = create_work_item()) == NULL)
|
||||
return -1; /* queues unlocked on failure */
|
||||
|
||||
work->func = dm_restart_func;
|
||||
work->restart_params.reason = reason;
|
||||
enqueue_work_item(work);
|
||||
|
||||
sem_wait(&work->wait_sem);
|
||||
int result = work->result;
|
||||
destroy_work_item(work);
|
||||
return result;
|
||||
}
|
||||
|
||||
static int
|
||||
task_main(int argc, char *argv[])
|
||||
{
|
||||
work_q_item_t *work;
|
||||
|
||||
/* inform about start */
|
||||
warnx("Initializing..");
|
||||
|
||||
/* Initialize global variables */
|
||||
g_key_offsets[0] = 0;
|
||||
|
||||
for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
|
||||
g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
|
||||
|
||||
unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
|
||||
|
||||
for (unsigned i = 0; i < dm_number_of_funcs; i++)
|
||||
g_func_counts[i] = 0;
|
||||
|
||||
g_task_should_exit = false;
|
||||
|
||||
init_q(&g_work_q);
|
||||
init_q(&g_free_q);
|
||||
|
||||
sem_init(&g_work_queued_sema, 1, 0);
|
||||
|
||||
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
|
||||
if (g_task_fd < 0) {
|
||||
warnx("Could not open data manager file %s", k_data_manager_device_path);
|
||||
return -1;
|
||||
}
|
||||
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
|
||||
close(g_task_fd);
|
||||
warnx("Could not seek data manager file %s", k_data_manager_device_path);
|
||||
return -1;
|
||||
}
|
||||
fsync(g_task_fd);
|
||||
|
||||
g_fd = g_task_fd;
|
||||
|
||||
warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
|
||||
|
||||
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) && (g_fd >= 0)) {
|
||||
/* Close the file handle to stop further queueing */
|
||||
g_fd = -1;
|
||||
}
|
||||
|
||||
if (!g_task_should_exit) {
|
||||
/* wait for work */
|
||||
sem_wait(&g_work_queued_sema);
|
||||
}
|
||||
|
||||
/* Empty the work queue */
|
||||
while ((work = dequeue_work_item())) {
|
||||
|
||||
switch (work->func) {
|
||||
case dm_write_func:
|
||||
g_func_counts[dm_write_func]++;
|
||||
work->result =
|
||||
_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
|
||||
break;
|
||||
|
||||
case dm_read_func:
|
||||
g_func_counts[dm_read_func]++;
|
||||
work->result =
|
||||
_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
|
||||
break;
|
||||
|
||||
case dm_clear_func:
|
||||
g_func_counts[dm_clear_func]++;
|
||||
work->result = _clear(work->clear_params.item);
|
||||
break;
|
||||
|
||||
case dm_restart_func:
|
||||
g_func_counts[dm_restart_func]++;
|
||||
work->result = _restart(work->restart_params.reason);
|
||||
break;
|
||||
|
||||
default: /* should never happen */
|
||||
work->result = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Inform the caller that work is done */
|
||||
sem_post(&work->wait_sem);
|
||||
}
|
||||
|
||||
/* time to go???? */
|
||||
if ((g_task_should_exit) && (g_fd < 0))
|
||||
break;
|
||||
}
|
||||
|
||||
close(g_task_fd);
|
||||
g_task_fd = -1;
|
||||
|
||||
/* Empty the work queue */
|
||||
for (;;) {
|
||||
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
|
||||
break;
|
||||
|
||||
free(work);
|
||||
}
|
||||
|
||||
destroy_q(&g_work_q);
|
||||
destroy_q(&g_free_q);
|
||||
sem_destroy(&g_work_queued_sema);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
start(void)
|
||||
{
|
||||
int task;
|
||||
|
||||
sem_init(&g_init_sema, 1, 0);
|
||||
|
||||
/* start the task */
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
|
||||
warn("task start failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* wait for the thread to actuall initialize */
|
||||
sem_wait(&g_init_sema);
|
||||
sem_destroy(&g_init_sema);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void
|
||||
status(void)
|
||||
{
|
||||
/* display usage statistics */
|
||||
warnx("Writes %d", g_func_counts[dm_write_func]);
|
||||
warnx("Reads %d", g_func_counts[dm_read_func]);
|
||||
warnx("Clears %d", g_func_counts[dm_clear_func]);
|
||||
warnx("Restarts %d", g_func_counts[dm_restart_func]);
|
||||
warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
|
||||
}
|
||||
|
||||
static void
|
||||
stop(void)
|
||||
{
|
||||
/* Tell the worker task to shut down */
|
||||
g_task_should_exit = true;
|
||||
sem_post(&g_work_queued_sema);
|
||||
}
|
||||
|
||||
static void
|
||||
usage(void)
|
||||
{
|
||||
errx(1, "usage: dataman {start|stop|status}");
|
||||
}
|
||||
|
||||
int
|
||||
dataman_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2)
|
||||
usage();
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (g_fd >= 0)
|
||||
errx(1, "already running");
|
||||
|
||||
start();
|
||||
|
||||
if (g_fd < 0)
|
||||
errx(1, "start failed");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (g_fd < 0)
|
||||
errx(1, "not running");
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
stop();
|
||||
else if (!strcmp(argv[1], "status"))
|
||||
status();
|
||||
else
|
||||
usage();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 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.h
|
||||
*
|
||||
* DATAMANAGER driver.
|
||||
*/
|
||||
#ifndef _DATAMANAGER_H
|
||||
#define _DATAMANAGER_H
|
||||
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <mavlink/waypoints.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_WAY_POINTS, /* Mission way point coordinates */
|
||||
DM_KEY_NUM_KEYS /* Total number of item types defined */
|
||||
} dm_item_t;
|
||||
|
||||
/* The maximum number of instances for each item type */
|
||||
enum {
|
||||
DM_KEY_SAFE_POINTS_MAX = 8,
|
||||
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
|
||||
DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT,
|
||||
};
|
||||
|
||||
/* Data persistence levels */
|
||||
typedef enum {
|
||||
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
|
||||
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
|
||||
DM_PERSIST_VOLATILE /* Data does not survive resets */
|
||||
} dm_persitence_t;
|
||||
|
||||
/* The reason for the last reset */
|
||||
typedef enum {
|
||||
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
|
||||
DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */
|
||||
} dm_reset_reason;
|
||||
|
||||
/* Maximum size in bytes of a single item instance */
|
||||
#define DM_MAX_DATA_SIZE 126
|
||||
|
||||
/* Retrieve from the data manager store */
|
||||
__EXPORT ssize_t
|
||||
dm_read(
|
||||
dm_item_t item, /* The item type to retrieve */
|
||||
unsigned char 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 char index, /* The index of the item */
|
||||
dm_persitence_t persistence, /* The persistence level of this item */
|
||||
const void *buffer, /* Pointer to caller data buffer */
|
||||
size_t buflen /* Length in bytes of data to retrieve */
|
||||
);
|
||||
|
||||
/* Retrieve from the data manager store */
|
||||
__EXPORT int
|
||||
dm_clear(
|
||||
dm_item_t item /* The item type to clear */
|
||||
);
|
||||
|
||||
/* Tell the data manager about the type of the last reset */
|
||||
__EXPORT int
|
||||
dm_restart(
|
||||
dm_reset_reason restart_type /* The last reset type */
|
||||
);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Main Navigation Controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = dataman
|
||||
|
||||
SRCS = dataman.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
Reference in New Issue
Block a user