mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 06:14:07 +08:00
POSIX Tests: Fix formatting
This commit is contained in:
parent
567935dedb
commit
480bc2f3c6
@ -49,8 +49,9 @@ int HelloExample::main()
|
||||
{
|
||||
appState.setRunning(true);
|
||||
|
||||
int i=0;
|
||||
while (!appState.exitRequested() && i<5) {
|
||||
int i = 0;
|
||||
|
||||
while (!appState.exitRequested() && i < 5) {
|
||||
sleep(2);
|
||||
|
||||
printf(" Doing work...\n");
|
||||
|
||||
@ -41,7 +41,8 @@
|
||||
|
||||
#include <px4_app.h>
|
||||
|
||||
class HelloExample {
|
||||
class HelloExample
|
||||
{
|
||||
public:
|
||||
HelloExample() {};
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ static int daemon_task; /* Handle of deamon task / thread */
|
||||
extern "C" __EXPORT int hello_main(int argc, char *argv[]);
|
||||
int hello_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
|
||||
if (argc < 2) {
|
||||
PX4_WARN("usage: hello {start|stop|status}\n");
|
||||
return 1;
|
||||
@ -70,11 +70,11 @@ int hello_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
daemon_task = px4_task_spawn_cmd("hello",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -54,6 +54,7 @@ static void timer_expired(void *arg)
|
||||
{
|
||||
static int i = 0;
|
||||
PX4_INFO("Test\n");
|
||||
|
||||
if (i < 5) {
|
||||
i++;
|
||||
hrt_call_after(&t1, update_interval, timer_expired, (void *)0);
|
||||
@ -79,7 +80,7 @@ int HRTTest::main()
|
||||
memset(&t1, 0, sizeof(t1));
|
||||
|
||||
PX4_INFO("HRT_CALL %d\n", hrt_called(&t1));
|
||||
|
||||
|
||||
hrt_call_after(&t1, update_interval, timer_expired, (void *)0);
|
||||
sleep(2);
|
||||
PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1));
|
||||
|
||||
@ -41,7 +41,8 @@
|
||||
|
||||
#include <px4_app.h>
|
||||
|
||||
class HRTTest {
|
||||
class HRTTest
|
||||
{
|
||||
public:
|
||||
HRTTest() {};
|
||||
|
||||
|
||||
@ -63,11 +63,11 @@ int hrttest_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
daemon_task = px4_task_spawn_cmd("hrttest",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -57,127 +57,128 @@ int MuorbTestExample::main()
|
||||
|
||||
int MuorbTestExample::DefaultTest()
|
||||
{
|
||||
int i=0;
|
||||
orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status );
|
||||
if( pub_id == 0 )
|
||||
{
|
||||
PX4_ERR( "error publishing esc_status" );
|
||||
return -1;
|
||||
}
|
||||
orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc );
|
||||
if( pub_id_vc == 0 )
|
||||
{
|
||||
PX4_ERR( "error publishing vehicle_command" );
|
||||
return -1;
|
||||
}
|
||||
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "[%d]Error publishing the vechile command message", i );
|
||||
return -1;
|
||||
}
|
||||
int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) );
|
||||
if ( sub_vc == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "Error subscribing to vehicle_command topic" );
|
||||
return -1;
|
||||
}
|
||||
int i = 0;
|
||||
orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status);
|
||||
|
||||
while (!appState.exitRequested() && i<100) {
|
||||
if (pub_id == 0) {
|
||||
PX4_ERR("error publishing esc_status");
|
||||
return -1;
|
||||
}
|
||||
|
||||
PX4_DEBUG("[%d] Doing work...", i );
|
||||
if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "[%d]Error publishing the esc status message for iter", i );
|
||||
break;
|
||||
}
|
||||
bool updated = false;
|
||||
if( orb_check( sub_vc, &updated ) == 0 )
|
||||
{
|
||||
if( updated )
|
||||
{
|
||||
PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i );
|
||||
if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 )
|
||||
{
|
||||
PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i );
|
||||
break;
|
||||
}
|
||||
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "[%d]Error publishing the vechile command message", i );
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PX4_DEBUG( "[%d] VC topic is not updated", i );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i );
|
||||
break;
|
||||
}
|
||||
|
||||
++i;
|
||||
}
|
||||
return 0;
|
||||
orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
|
||||
|
||||
if (pub_id_vc == 0) {
|
||||
PX4_ERR("error publishing vehicle_command");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
|
||||
PX4_ERR("[%d]Error publishing the vechile command message", i);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
if (sub_vc == PX4_ERROR) {
|
||||
PX4_ERR("Error subscribing to vehicle_command topic");
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (!appState.exitRequested() && i < 100) {
|
||||
|
||||
PX4_DEBUG("[%d] Doing work...", i);
|
||||
|
||||
if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) {
|
||||
PX4_ERR("[%d]Error publishing the esc status message for iter", i);
|
||||
break;
|
||||
}
|
||||
|
||||
bool updated = false;
|
||||
|
||||
if (orb_check(sub_vc, &updated) == 0) {
|
||||
if (updated) {
|
||||
PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
|
||||
|
||||
if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
|
||||
PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
|
||||
break;
|
||||
}
|
||||
|
||||
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
|
||||
PX4_ERR("[%d]Error publishing the vechile command message", i);
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("[%d] VC topic is not updated", i);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("[%d]Error checking the updated status for vechile command... ", i);
|
||||
break;
|
||||
}
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MuorbTestExample::PingPongTest()
|
||||
{
|
||||
int i=0;
|
||||
orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc );
|
||||
if( pub_id_vc == 0 )
|
||||
{
|
||||
PX4_ERR( "error publishing vehicle_command" );
|
||||
return -1;
|
||||
}
|
||||
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "[%d]Error publishing the vechile command message", i );
|
||||
return -1;
|
||||
}
|
||||
int sub_esc_status = orb_subscribe( ORB_ID( esc_status ) );
|
||||
if ( sub_esc_status == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "Error subscribing to esc_status topic" );
|
||||
return -1;
|
||||
}
|
||||
int i = 0;
|
||||
orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
|
||||
|
||||
while (!appState.exitRequested() ) {
|
||||
if (pub_id_vc == 0) {
|
||||
PX4_ERR("error publishing vehicle_command");
|
||||
return -1;
|
||||
}
|
||||
|
||||
PX4_INFO("[%d] Doing work...", i );
|
||||
bool updated = false;
|
||||
if( orb_check( sub_esc_status, &updated ) == 0 )
|
||||
{
|
||||
if( updated )
|
||||
{
|
||||
PX4_INFO( "[%d]ESC status is updated... reading new value", i );
|
||||
if( orb_copy( ORB_ID( esc_status ), sub_esc_status, &m_esc_status ) != 0 )
|
||||
{
|
||||
PX4_ERR( "[%d]Error calling orb copy for esc status... ", i );
|
||||
break;
|
||||
}
|
||||
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
|
||||
{
|
||||
PX4_ERR( "[%d]Error publishing the vechile command message", i );
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PX4_INFO( "[%d] esc status topic is not updated", i );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PX4_ERR( "[%d]Error checking the updated status for esc status... ", i );
|
||||
break;
|
||||
}
|
||||
// sleep for 1 sec.
|
||||
usleep( 1000000 );
|
||||
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
|
||||
PX4_ERR("[%d]Error publishing the vechile command message", i);
|
||||
return -1;
|
||||
}
|
||||
|
||||
++i;
|
||||
}
|
||||
return 0;
|
||||
int sub_esc_status = orb_subscribe(ORB_ID(esc_status));
|
||||
|
||||
if (sub_esc_status == PX4_ERROR) {
|
||||
PX4_ERR("Error subscribing to esc_status topic");
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (!appState.exitRequested()) {
|
||||
|
||||
PX4_INFO("[%d] Doing work...", i);
|
||||
bool updated = false;
|
||||
|
||||
if (orb_check(sub_esc_status, &updated) == 0) {
|
||||
if (updated) {
|
||||
PX4_INFO("[%d]ESC status is updated... reading new value", i);
|
||||
|
||||
if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) {
|
||||
PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
|
||||
break;
|
||||
}
|
||||
|
||||
if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
|
||||
PX4_ERR("[%d]Error publishing the vechile command message", i);
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("[%d] esc status topic is not updated", i);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("[%d]Error checking the updated status for esc status... ", i);
|
||||
break;
|
||||
}
|
||||
|
||||
// sleep for 1 sec.
|
||||
usleep(1000000);
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -43,7 +43,8 @@
|
||||
#include "uORB/topics/esc_status.h"
|
||||
#include "uORB/topics/vehicle_command.h"
|
||||
|
||||
class MuorbTestExample {
|
||||
class MuorbTestExample
|
||||
{
|
||||
public:
|
||||
MuorbTestExample() {};
|
||||
|
||||
@ -53,9 +54,9 @@ public:
|
||||
|
||||
static px4::AppState appState; /* track requests to terminate app */
|
||||
private:
|
||||
int DefaultTest();
|
||||
int PingPongTest();
|
||||
struct esc_status_s m_esc_status;
|
||||
struct vehicle_command_s m_vc;
|
||||
|
||||
int DefaultTest();
|
||||
int PingPongTest();
|
||||
struct esc_status_s m_esc_status;
|
||||
struct vehicle_command_s m_vc;
|
||||
|
||||
};
|
||||
|
||||
@ -43,14 +43,14 @@
|
||||
#include "muorb_test_example.h"
|
||||
#include <stdio.h>
|
||||
#include "uORB/uORBManager.hpp"
|
||||
#include "uORBKraitFastRpcChannel.hpp"
|
||||
#include "uORBKraitFastRpcChannel.hpp"
|
||||
|
||||
int PX4_MAIN(int argc, char **argv)
|
||||
{
|
||||
px4::init(argc, argv, "muorb_test");
|
||||
|
||||
PX4_DEBUG("muorb_test");
|
||||
|
||||
|
||||
MuorbTestExample hello;
|
||||
hello.main();
|
||||
|
||||
|
||||
@ -71,11 +71,11 @@ int muorb_test_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
daemon_task = px4_task_spawn_cmd("muorb_test",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
16000,
|
||||
PX4_MAIN,
|
||||
(char* const*)argv);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
16000,
|
||||
PX4_MAIN,
|
||||
(char *const *)argv);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -60,44 +60,50 @@ 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;
|
||||
int i=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);
|
||||
buf[0] = 'A' + (char)(i % 26);
|
||||
PX4_INFO("Writer: writing char '%c'", buf[0]);
|
||||
ret = px4_write(fd, buf, 1);
|
||||
++i;
|
||||
++i;
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
PX4_INFO("Writer: stopped");
|
||||
return ret;
|
||||
}
|
||||
|
||||
class PrivData {
|
||||
class PrivData
|
||||
{
|
||||
public:
|
||||
PrivData() : _read_offset(0) {}
|
||||
~PrivData() {}
|
||||
|
||||
|
||||
size_t _read_offset;
|
||||
};
|
||||
|
||||
class VCDevNode : public VDev {
|
||||
|
||||
class VCDevNode : public VDev
|
||||
{
|
||||
public:
|
||||
VCDevNode() :
|
||||
VDev("vcdevtest", TESTDEV),
|
||||
VCDevNode() :
|
||||
VDev("vcdevtest", TESTDEV),
|
||||
_is_open_for_write(false),
|
||||
_write_offset(0) {};
|
||||
|
||||
@ -120,22 +126,25 @@ int VCDevNode::open(device::file_t *handlep)
|
||||
errno = EBUSY;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = VDev::open(handlep);
|
||||
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
handlep->priv = new PrivData;
|
||||
|
||||
if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) {
|
||||
_is_open_for_write = true;
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int VCDevNode::close(device::file_t *handlep)
|
||||
{
|
||||
delete (PrivData *)handlep->priv;
|
||||
delete(PrivData *)handlep->priv;
|
||||
handlep->priv = nullptr;
|
||||
VDev::close(handlep);
|
||||
|
||||
@ -143,12 +152,13 @@ int VCDevNode::close(device::file_t *handlep)
|
||||
if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) {
|
||||
_is_open_for_write = false;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen)
|
||||
{
|
||||
for (size_t i=0; i<buflen && _write_offset<1000; i++) {
|
||||
for (size_t i = 0; i < buflen && _write_offset < 1000; i++) {
|
||||
_buf[_write_offset] = buffer[i];
|
||||
_write_offset++;
|
||||
}
|
||||
@ -164,16 +174,18 @@ ssize_t VCDevNode::read(device::file_t *handlep, char *buffer, size_t buflen)
|
||||
PrivData *p = (PrivData *)handlep->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++) {
|
||||
|
||||
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() {
|
||||
VCDevExample::~VCDevExample()
|
||||
{
|
||||
if (_node) {
|
||||
delete _node;
|
||||
_node = 0;
|
||||
@ -183,16 +195,19 @@ VCDevExample::~VCDevExample() {
|
||||
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;
|
||||
@ -214,32 +229,39 @@ int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after
|
||||
// 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 {
|
||||
|
||||
} else {
|
||||
PX4_INFO("Reader: read failed %d FAIL", readret);
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
||||
} 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);
|
||||
usleep(delayms_after_poll * 1000);
|
||||
}
|
||||
|
||||
loop_count++;
|
||||
}
|
||||
|
||||
return 0;
|
||||
fail:
|
||||
return 1;
|
||||
@ -269,47 +291,61 @@ int VCDevExample::main()
|
||||
|
||||
void *p = 0;
|
||||
int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno);
|
||||
return -px4_errno;
|
||||
}
|
||||
|
||||
PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL");
|
||||
|
||||
ret = test_pub_block(fd, 1);
|
||||
if (ret < 0)
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = test_pub_block(fd, 0);
|
||||
if (ret < 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*)NULL);
|
||||
(void)px4_task_spawn_cmd("writer",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 6,
|
||||
2000,
|
||||
writer_main,
|
||||
(char *const *)NULL);
|
||||
|
||||
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)) {
|
||||
|
||||
if (do_poll(fd, 0, 3, 0)) {
|
||||
ret = 1;
|
||||
goto fail2;
|
||||
}
|
||||
|
||||
PX4_INFO("TEST: 100ms TIMEOUT POLL -----------");
|
||||
if(do_poll(fd, 0, 30, 100)) {
|
||||
|
||||
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)) {
|
||||
|
||||
if (do_poll(fd, 1000, 3, 0)) {
|
||||
ret = 1;
|
||||
goto fail2;
|
||||
}
|
||||
|
||||
@ -43,7 +43,8 @@
|
||||
|
||||
class VCDevNode;
|
||||
|
||||
class VCDevExample {
|
||||
class VCDevExample
|
||||
{
|
||||
public:
|
||||
VCDevExample() : _node(0) {};
|
||||
|
||||
|
||||
@ -50,7 +50,7 @@ static int daemon_task; /* Handle of deamon task / thread */
|
||||
extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]);
|
||||
int vcdevtest_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
|
||||
if (argc < 2) {
|
||||
printf("usage: vcdevtest {start|stop|status}\n");
|
||||
return 1;
|
||||
@ -65,11 +65,11 @@ int vcdevtest_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
daemon_task = px4_task_spawn_cmd("vcdevtest",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -52,7 +52,7 @@ static int daemon_task; /* Handle of deamon task / thread */
|
||||
extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]);
|
||||
int wqueue_test_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
|
||||
if (argc < 2) {
|
||||
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
|
||||
return 1;
|
||||
@ -67,11 +67,11 @@ int wqueue_test_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
daemon_task = px4_task_spawn_cmd("wqueue",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2000,
|
||||
PX4_MAIN,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -65,8 +65,11 @@ void WQueueTest::do_lp_work()
|
||||
{
|
||||
static int iter = 0;
|
||||
printf("done lp work\n");
|
||||
if (iter > 5)
|
||||
|
||||
if (iter > 5) {
|
||||
_lpwork_done = true;
|
||||
}
|
||||
|
||||
++iter;
|
||||
|
||||
work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000);
|
||||
@ -76,8 +79,11 @@ void WQueueTest::do_hp_work()
|
||||
{
|
||||
static int iter = 0;
|
||||
printf("done hp work\n");
|
||||
if (iter > 5)
|
||||
|
||||
if (iter > 5) {
|
||||
_hpwork_done = true;
|
||||
}
|
||||
|
||||
++iter;
|
||||
|
||||
// requeue
|
||||
|
||||
@ -43,11 +43,12 @@
|
||||
#include <px4_workqueue.h>
|
||||
#include <string.h>
|
||||
|
||||
class WQueueTest {
|
||||
class WQueueTest
|
||||
{
|
||||
public:
|
||||
WQueueTest() :
|
||||
_lpwork_done(false),
|
||||
_hpwork_done(false)
|
||||
WQueueTest() :
|
||||
_lpwork_done(false),
|
||||
_hpwork_done(false)
|
||||
{
|
||||
memset(&_lpwork, 0, sizeof(_lpwork));
|
||||
memset(&_hpwork, 0, sizeof(_hpwork));
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user