forked from Archive/PX4-Autopilot
Merge branch 'beta' of github.com:PX4/Firmware
This commit is contained in:
commit
87dfdef69b
|
@ -1,8 +1,10 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier
|
||||
* Jean Cyr
|
||||
* Author: Jean Cyr
|
||||
* Lorenz Meier
|
||||
* Julian Oes
|
||||
* Thomas Gubler
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -40,16 +42,8 @@
|
|||
#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"
|
||||
|
@ -175,8 +169,10 @@ create_work_item(void)
|
|||
|
||||
/* 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 one */
|
||||
|
@ -339,6 +335,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
|
|||
|
||||
/* 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);
|
||||
|
||||
|
@ -599,17 +596,20 @@ task_main(int argc, char *argv[])
|
|||
|
||||
/* Open or create the data manager file */
|
||||
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);
|
||||
sem_post(&g_init_sema); /* Don't want to hang startup */
|
||||
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);
|
||||
sem_post(&g_init_sema); /* Don't want to hang startup */
|
||||
return -1;
|
||||
}
|
||||
|
||||
fsync(g_task_fd);
|
||||
|
||||
/* We use two file descriptors, one for the caller context and one for the worker thread */
|
||||
|
|
|
@ -38,5 +38,3 @@
|
|||
MODULE_COMMAND = dataman
|
||||
|
||||
SRCS = dataman.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
|
Loading…
Reference in New Issue