fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 03:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

make vcdevtest a generic cdev test

Browse Source
This commit is contained in:
Daniel Agar
2018-11-24 11:37:52 -05:00
committed by David Sidrane
parent 3e0a3559a9
commit c3448c19c4
8 changed files with 74 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/tests/vcdev_test/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE platforms__posix__tests__vcdev_test
MAIN vcdev_test
SRCS
vcdevtest_main.cpp
vcdevtest_start_posix.cpp
vcdevtest_example.cpp
DEPENDS
)
src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
-364
View File
@@ -1,364 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 vcdevtest_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_tasks.h>
#include <px4_time.h>
#include "vcdevtest_example.h"
#include <drivers/drv_device.h>
#include <lib/cdev/CDev.hpp>
#include <unistd.h>
#include <stdio.h>
px4::AppState VCDevExample::appState;
#define TESTDEV "/dev/vdevtest"
static bool g_exit = false;
static int writer_main(int argc, char *argv[])
{
char buf[1];
int fd = px4_open(TESTDEV, PX4_F_WRONLY);
if (fd < 0) {
PX4_INFO("Writer: Open failed %d %d", fd, px4_errno);
return -px4_errno;
}
int ret = 0;
int i = 0;
while (!g_exit) {
// Wait for 2 seconds
PX4_INFO("Writer: Sleeping for 2 sec");
ret = sleep(2);
if (ret < 0) {
PX4_INFO("Writer: sleep failed %d %d", ret, errno);
return ret;
}
buf[0] = 'A' + (char)(i % 26);
PX4_INFO("Writer: writing char '%c'", buf[0]);
ret = px4_write(fd, buf, 1);
++i;
}
px4_close(fd);
PX4_INFO("Writer: stopped");
return ret;
}
class PrivData
{
public:
PrivData() : _read_offset(0) {}
~PrivData() = default;
size_t _read_offset;
};
class VCDevNode : public cdev::CDev
{
public:
VCDevNode() :
CDev(TESTDEV),
_is_open_for_write(false),
_write_offset(0) {}
~VCDevNode() = default;
virtual int open(cdev::file_t *handlep);
virtual int close(cdev::file_t *handlep);
virtual ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen);
virtual ssize_t read(cdev::file_t *handlep, char *buffer, size_t buflen);
private:
bool _is_open_for_write;
size_t _write_offset;
char _buf[1000];
};
int VCDevNode::open(cdev::file_t *handlep)
{
// Only allow one writer
if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) {
errno = EBUSY;
return -1;
}
int ret = CDev::open(handlep);
if (ret != 0) {
return ret;
}
handlep->f_priv = new PrivData;
if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) {
_is_open_for_write = true;
}
return 0;
}
int VCDevNode::close(cdev::file_t *handlep)
{
delete (PrivData *)handlep->f_priv;
handlep->f_priv = nullptr;
CDev::close(handlep);
// Enable a new writer of the device is re-opened for write
if ((handlep->f_oflags & PX4_F_WRONLY) && _is_open_for_write) {
_is_open_for_write = false;
}
return 0;
}
ssize_t VCDevNode::write(cdev::file_t *handlep, const char *buffer, size_t buflen)
{
for (size_t i = 0; i < buflen && _write_offset < 1000; i++) {
_buf[_write_offset] = buffer[i];
_write_offset++;
}
// ignore what was written, but let pollers know something was written
poll_notify(POLLIN);
return buflen;
}
ssize_t VCDevNode::read(cdev::file_t *handlep, char *buffer, size_t buflen)
{
PrivData *p = (PrivData *)handlep->f_priv;
ssize_t chars_read = 0;
PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset);
for (size_t i = 0; i < buflen && (p->_read_offset < _write_offset); i++) {
buffer[i] = _buf[p->_read_offset];
p->_read_offset++;
chars_read++;
}
return chars_read;
}
VCDevExample::~VCDevExample()
{
if (_node) {
delete _node;
_node = nullptr;
}
}
static int test_pub_block(int fd, unsigned long blocked)
{
int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked);
if (ret < 0) {
PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno);
return -px4_errno;
}
ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0);
if (ret < 0) {
PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno);
return -px4_errno;
}
PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
return 0;
}
int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll)
{
int pollret, readret;
int loop_count = 0;
char readbuf[10];
px4_pollfd_struct_t fds[1];
fds[0].fd = fd;
fds[0].events = POLLIN;
fds[0].revents = 0;
bool mustblock = (timeout < 0);
// Test indefinte blocking poll
while ((!appState.exitRequested()) && (loop_count < iterations)) {
pollret = px4_poll(fds, 1, timeout);
if (pollret < 0) {
PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno);
goto fail;
}
PX4_INFO("Reader: px4_poll returned %d", pollret);
if (pollret) {
readret = px4_read(fd, readbuf, 10);
if (readret != 1) {
if (mustblock) {
PX4_ERR("Reader: read failed %d FAIL", readret);
goto fail;
} else {
PX4_INFO("Reader: read failed %d FAIL", readret);
}
} else {
readbuf[readret] = '\0';
PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf);
}
}
if (delayms_after_poll) {
usleep(delayms_after_poll * 1000);
}
loop_count++;
}
return 0;
fail:
return 1;
}
int VCDevExample::main()
{
appState.setRunning(true);
_node = new VCDevNode();
if (_node == nullptr) {
PX4_INFO("Failed to allocate VCDevNode");
return -ENOMEM;
}
if (_node->init() != PX4_OK) {
PX4_INFO("Failed to init VCDevNode");
return 1;
}
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
if (fd < 0) {
PX4_INFO("Open failed %d %d", fd, px4_errno);
return -px4_errno;
}
int ret = test_pub_block(fd, 1);
if (ret < 0) {
return ret;
}
ret = test_pub_block(fd, 0);
if (ret < 0) {
return ret;
}
// Start a task that will write something in 4 seconds
(void)px4_task_spawn_cmd("writer",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
2000,
writer_main,
(char *const *)nullptr);
ret = 0;
PX4_INFO("TEST: BLOCKING POLL ---------------");
if (do_poll(fd, -1, 3, 0)) {
ret = 1;
goto fail2;
}
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
if (do_poll(fd, 0, 3, 0)) {
ret = 1;
goto fail2;
goto fail2;
}
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
if (do_poll(fd, 0, 3, 0)) {
ret = 1;
goto fail2;
goto fail2;
}
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
if (do_poll(fd, 0, 3, 0)) {
ret = 1;
goto fail2;
}
PX4_INFO("TEST: 100ms TIMEOUT POLL -----------");
if (do_poll(fd, 0, 30, 100)) {
ret = 1;
goto fail2;
}
PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
if (do_poll(fd, 1000, 3, 0)) {
ret = 1;
goto fail2;
}
PX4_INFO("TEST: waiting for writer to stop");
fail2:
g_exit = true;
px4_close(fd);
PX4_INFO("TEST: waiting for writer to stop");
sleep(3);
return ret;
}
src/platforms/posix/tests/vcdev_test/vcdevtest_example.h
-61
View File
@@ -1,61 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 vcdevtest_example.h
* Example app for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
class VCDevNode;
class VCDevExample
{
public:
VCDevExample() : _node(0) {}
~VCDevExample();
int main();
static px4::AppState appState; /* track requests to terminate app */
private:
int do_poll(int fd, int timeout, int iterations, int delayms_after_poll);
VCDevNode *_node;
};
src/platforms/posix/tests/vcdev_test/vcdevtest_main.cpp
-55
View File
@@ -1,55 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 vcdevtest_main.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_app.h>
#include "vcdevtest_example.h"
#include <stdio.h>
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "vcdevtest");
printf("vcdevtest\n");
VCDevExample vcdevtest;
vcdevtest.main();
printf("goodbye\n");
return 0;
}
src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp
-95
View File
@@ -1,95 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 vcdevtest_start_posix.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "vcdevtest_example.h"
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int vcdev_test_main(int argc, char *argv[]);
int vcdev_test_main(int argc, char *argv[])
{
if (argc < 2) {
printf("usage: vcdevtest {start|stop|status}\n");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (VCDevExample::appState.isRunning()) {
printf("already running\n");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("vcdevtest",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
PX4_MAIN,
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
return 0;
}
if (!strcmp(argv[1], "stop")) {
VCDevExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (VCDevExample::appState.isRunning()) {
printf("is running\n");
} else {
printf("not started\n");
}
return 0;
}
printf("usage: vcdevtest_main {start|stop|status}\n");
return 1;
}
src/platforms/px4_app.h
+2 -1
View File
@@ -73,9 +73,10 @@ private:
}
// Task/process based build
#if defined(__PX4_ROS) || defined(__PX4_NUTTX)
#if defined(__PX4_ROS)
// Thread based build
#else
#ifdef PX4_MAIN