dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'jean-m-cyr/master' into navigator_wip

This commit is contained in:
Julian Oes 2013-11-18 13:00:09 +01:00
parent 39634d1001 1cf9f72f62
commit 29578a5604
15 changed files with 1470 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -105,6 +105,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -102,6 +102,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries

22
src/lib/geo/geo.c
View File

@ -465,4 +465,26 @@ __EXPORT float _wrap_360(float bearing)
return bearing;
}
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence)
{
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
unsigned int i, j, vertices = fence->count;
bool c = false;
double lat = vehicle->lat / 1e7;
double lon = vehicle->lon / 1e7;
// skip vertex 0 (return point)
for (i = 0, j = vertices - 1; i < vertices; j = i++)
if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) &&
(lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) /
(fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat))
c = !c;
return c;
}

14
src/lib/geo/geo.h
View File

@ -47,6 +47,9 @@
#pragma once
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
__BEGIN_DECLS
#include <stdbool.h>
@ -126,4 +129,15 @@ __EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
/**
* Return whether craft is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
*/
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence);
__END_DECLS

739
src/modules/dataman/dataman.c Normal file
View File

@ -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;
}

116
src/modules/dataman/dataman.h Normal file
View File

@ -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

42
src/modules/dataman/module.mk Normal file
View File

@ -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

2
src/modules/navigator/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink

325
src/modules/navigator/navigator_main.cpp
View File

@ -2,6 +2,7 @@
*
* 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
@ -48,6 +49,8 @@
#include <math.h>
#include <poll.h>
#include <time.h>
#include <sys/ioctl.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
@ -62,12 +65,14 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <dataman/dataman.h>
/**
* navigator app start / stop handling function
@ -90,12 +95,27 @@ public:
~Navigator();
/**
* Start the sensors task.
* Start the navigator task.
*
* @return OK on success.
*/
int start();
/**
* Display the navigator status.
*/
void status();
/**
* Load fence parameters.
*/
bool load_fence(unsigned vertices);
/**
* Specify fence vertex position.
*/
void fence_point(int argc, char *argv[]);
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@ -108,7 +128,10 @@ private:
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _mission_sub;
int _capabilities_sub;
int _fence_sub;
int _fence_pub;
orb_advert_t _triplet_pub; /**< position setpoint */
@ -122,9 +145,16 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
unsigned _mission_items_maxcount; /**< maximum number of mission items supported */
struct mission_item_s * _mission_items; /**< storage for mission items */
bool _mission_valid; /**< flag if mission is valid */
unsigned _mission_items_maxcount; /**< maximum number of mission items supported */
struct mission_item_s * _mission_items; /**< storage for mission items */
bool _mission_valid; /**< flag if mission is valid */
struct fence_s _fence; /**< storage for fence vertices */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
struct navigation_capabilities_s _nav_caps;
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
@ -169,6 +199,11 @@ private:
*/
void mission_poll();
/**
* Retrieve mission.
*/
void mission_update();
/**
* Control throttle.
*/
@ -192,6 +227,14 @@ private:
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void publish_fence(unsigned vertices);
void publish_mission(unsigned points);
void publish_safepoints(unsigned points);
bool fence_valid(const struct fence_s &fence);
};
namespace navigator
@ -218,6 +261,10 @@ Navigator::Navigator() :
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_fence_sub(-1),
_fence_pub(-1),
_mission_sub(-1),
_capabilities_sub(-1),
/* publications */
_triplet_pub(-1),
@ -227,17 +274,15 @@ Navigator::Navigator() :
/* states */
_mission_items_maxcount(20),
_mission_valid(false),
_loiter_hold(false)
_loiter_hold(false),
_fence_valid(false),
_inside_fence(true)
{
_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
if (!_mission_items) {
_mission_items_maxcount = 0;
warnx("no free RAM to allocate mission, rejecting any waypoints");
}
_global_pos.valid = false;
memset(&_fence, 0, sizeof(_fence));
_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
/* fetch initial parameter values */
/* fetch initial values */
parameters_update();
}
@ -283,10 +328,8 @@ Navigator::vehicle_status_poll()
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
if (vstatus_updated) {
if (vstatus_updated)
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
}
}
void
@ -296,9 +339,8 @@ Navigator::vehicle_attitude_poll()
bool att_updated;
orb_check(_att_sub, &att_updated);
if (att_updated) {
if (att_updated)
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
}
void
@ -308,18 +350,22 @@ Navigator::mission_poll()
bool mission_updated;
orb_check(_mission_sub, &mission_updated);
if (mission_updated) {
struct mission_s mission;
orb_copy(ORB_ID(mission), _mission_sub, &mission);
if (mission_updated)
mission_update();
}
void
Navigator::mission_update()
{
struct mission_s mission;
if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) {
// XXX this is not optimal yet, but a first prototype /
// test implementation
if (mission.count <= _mission_items_maxcount) {
/*
* Perform an atomic copy & state update
*/
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
@ -351,12 +397,22 @@ Navigator::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_sub = orb_subscribe(ORB_ID(mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_fence_sub = orb_subscribe(ORB_ID(fence));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
// Load initial states
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
_vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running
}
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
mission_update();
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
/* rate limit position updates to 50 Hz */
@ -364,23 +420,35 @@ Navigator::task_main()
parameters_update();
_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
/* load the craft capabilities */
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
/* wakeup source(s) */
struct pollfd fds[2];
struct pollfd fds[5];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
fds[2].fd = _fence_sub;
fds[2].events = POLLIN;
fds[3].fd = _capabilities_sub;
fds[3].events = POLLIN;
fds[4].fd = _mission_sub;
fds[4].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@ -403,17 +471,34 @@ Navigator::task_main()
parameters_update();
}
/* only update fence if it has changed */
if (fds[2].revents & POLLIN) {
/* read from fence to clear updated flag */
unsigned vertices;
orb_copy(ORB_ID(fence), _fence_sub, &vertices);
_fence_valid = load_fence(vertices);
}
/* only update craft capabilities if they have changed */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
if (fds[4].revents & POLLIN) {
mission_update();
}
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (deltaT > 1.0f) {
deltaT = 0.01f;
}
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@ -422,6 +507,10 @@ Navigator::task_main()
mission_poll();
if (_fence_valid && _global_pos.valid) {
_inside_fence = inside_geofence(&_global_pos, &_fence);
}
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
// Current waypoint
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
@ -460,14 +549,14 @@ Navigator::task_main()
if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
/* waypoint is a plain navigation waypoint */
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
/* waypoint is a loiter waypoint */
}
// XXX at this point we always want no loiter hold if a
@ -528,9 +617,10 @@ Navigator::task_main()
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
warnx("exiting.");
_navigator_task = -1;
_exit(0);
@ -543,11 +633,11 @@ Navigator::start()
/* start the task */
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&Navigator::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&Navigator::task_main_trampoline,
nullptr);
if (_navigator_task < 0) {
warn("task start failed");
@ -557,20 +647,139 @@ Navigator::start()
return OK;
}
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
if (_global_pos.valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)_global_pos.relative_alt);
warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f",
(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795);
}
if (_fence_valid) {
warnx("Geofence is valid");
warnx("Vertex longitude latitude");
for (unsigned i = 0; i < _fence.count; i++)
warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else
warnx("Geofence not set");
}
void
Navigator::publish_fence(unsigned vertices)
{
if (_fence_pub == -1)
_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
else
orb_publish(ORB_ID(fence), _fence_pub, &vertices);
}
bool
Navigator::fence_valid(const struct fence_s &fence)
{
struct vehicle_global_position_s pos;
// NULL fence is valid
if (fence.count == 0) {
return true;
}
// Otherwise
if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
return false;
}
return true;
}
bool
Navigator::load_fence(unsigned vertices)
{
struct fence_s temp_fence;
unsigned i;
for (i = 0; i < vertices; i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
}
temp_fence.count = i;
if (fence_valid(temp_fence))
memcpy(&_fence, &temp_fence, sizeof(_fence));
else
warnx("Invalid fence file, ignored!");
return _fence.count != 0;
}
void
Navigator::fence_point(int argc, char *argv[])
{
int ix, last;
double lon, lat;
struct fence_vertex_s vertex;
char *end;
if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
dm_clear(DM_KEY_FENCE_POINTS);
publish_fence(0);
return;
}
if (argc < 3)
errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
ix = atoi(argv[0]);
if (ix >= DM_KEY_FENCE_POINTS_MAX)
errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
lat = strtod(argv[1], &end);
lon = strtod(argv[2], &end);
last = 0;
if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
last = 1;
vertex.lat = (float)lat;
vertex.lon = (float)lon;
if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
if (last)
publish_fence((unsigned)ix + 1);
return;
}
errx(1, "can't store fence point");
}
static void usage()
{
errx(1, "usage: navigator {start|stop|status|fence}");
}
int navigator_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: navigator {start|stop|status}");
if (argc < 2) {
usage();
}
if (!strcmp(argv[1], "start")) {
if (navigator::g_navigator != nullptr)
if (navigator::g_navigator != nullptr) {
errx(1, "already running");
}
navigator::g_navigator = new Navigator;
if (navigator::g_navigator == nullptr)
if (navigator::g_navigator == nullptr) {
errx(1, "alloc failed");
}
if (OK != navigator::g_navigator->start()) {
delete navigator::g_navigator;
@ -578,27 +787,25 @@ int navigator_main(int argc, char *argv[])
err(1, "start failed");
}
exit(0);
return 0;
}
if (navigator::g_navigator == nullptr)
errx(1, "not running");
if (!strcmp(argv[1], "stop")) {
if (navigator::g_navigator == nullptr)
errx(1, "not running");
delete navigator::g_navigator;
navigator::g_navigator = nullptr;
exit(0);
} else if (!strcmp(argv[1], "status")) {
navigator::g_navigator->status();
} else if (!strcmp(argv[1], "fence")) {
navigator::g_navigator->fence_point(argc - 2, argv + 2);
} else {
usage();
}
if (!strcmp(argv[1], "status")) {
if (navigator::g_navigator) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
return 0;
}

3
src/modules/uORB/objects_common.cpp
View File

@ -129,6 +129,9 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
#include "topics/fence.h"
ORB_DEFINE(fence, unsigned);
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);

80
src/modules/uORB/topics/fence.h Normal file
View File

@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Jean Cyr <jean.m.cyr@gmail.com>
*
* 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 fence.h
* Definition of geofence.
*/
#ifndef TOPIC_FENCE_H_
#define TOPIC_FENCE_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
#define GEOFENCE_MAX_VERTICES 16
/**
* This is the position of a geofence vertex
*
*/
struct fence_vertex_s {
// Worst case float precision gives us 2 meter resolution at the equator
float lat; /**< latitude in degrees */
float lon; /**< longitude in degrees */
};
/**
* This is the position of a geofence
*
*/
struct fence_s {
unsigned count; /**< number of actual vertices */
struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(fence);
#endif

3
src/systemcmds/tests/module.mk
View File

@ -24,6 +24,9 @@ SRCS = test_adc.c \
test_uart_loopback.c \
test_uart_send.c \
test_mixer.cpp \
test_dataman.c \
tests_file.c \
tests_main.c \
tests_param.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink

179
src/systemcmds/tests/test_dataman.c Normal file
View File

@ -0,0 +1,179 @@
/****************************************************************************
* px4/sensors/test_dataman.c
*
* Copyright (C) 2012 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <drivers/drv_led.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <semaphore.h>
#include "tests.h"
#include "dataman/dataman.h"
static sem_t *sems;
static int
task_main(int argc, char *argv[])
{
char buffer[DM_MAX_DATA_SIZE];
hrt_abstime wstart, wend, rstart, rend;
warnx("Starting dataman test task %s", argv[1]);
/* try to read an invalid item */
int my_id = atoi(argv[1]);
/* try to read an invalid item */
if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
warnx("%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) {
warnx("%d read an invalid index failed", my_id);
goto fail;
}
srand(hrt_absolute_time() ^ my_id);
unsigned hit = 0, miss = 0;
wstart = hrt_absolute_time();
for (unsigned i = 0; i < 256; i++) {
memset(buffer, my_id, sizeof(buffer));
buffer[1] = i;
unsigned hash = i ^ my_id;
unsigned len = (hash & 63) + 2;
if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) {
warnx("%d write failed, index %d, length %d", my_id, hash, len);
goto fail;
}
usleep(rand() & ((64 * 1024) - 1));
}
rstart = hrt_absolute_time();
wend = rstart;
for (unsigned i = 0; i < 256; i++) {
unsigned hash = i ^ my_id;
unsigned len2, len = (hash & 63) + 2;
if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) {
warnx("%d read failed length test, index %d", my_id, hash);
goto fail;
}
if (buffer[0] == my_id) {
hit++;
if (len2 != len) {
warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
goto fail;
}
if (buffer[1] != i) {
warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
goto fail;
}
}
else
miss++;
}
rend = hrt_absolute_time();
warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000);
sem_post(sems + my_id);
return 0;
fail:
warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
sem_post(sems + my_id);
return -1;
}
int test_dataman(int argc, char *argv[])
{
int i, num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1)
num_tasks = atoi(argv[1]);
sems = (sem_t *)malloc(num_tasks * sizeof(sem_t));
warnx("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
sprintf(a, "%d", i);
const char *av[2];
av[0] = a;
av[1] = 0;
sem_init(sems + i, 1, 0);
/* start the task */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
warn("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
sem_wait(sems + i);
sem_destroy(sems + i);
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < 256; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0)
break;
}
if (i >= 256) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < 256; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}
}
return 0;
}

1
src/systemcmds/tests/tests.h
View File

@ -101,6 +101,7 @@ extern int test_param(int argc, char *argv[]);
extern int test_bson(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]);
extern int test_mixer(int argc, char *argv[]);
extern int test_dataman(int argc, char *argv[]);
__END_DECLS

1
src/systemcmds/tests/tests_main.c
View File

@ -112,6 +112,7 @@ const struct {
{"file", test_file, 0},
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{"dataman", test_dataman, OPT_NOALLTEST},
{NULL, NULL, 0}
};