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