forked from Archive/PX4-Autopilot
Merge branch 'qurt' into linux
This commit is contained in:
commit
2d32395bc0
|
@ -72,7 +72,7 @@ MODULES += platforms/posix/drivers/barosim
|
||||||
# Unit tests
|
# Unit tests
|
||||||
#
|
#
|
||||||
MODULES += platforms/qurt/tests/hello
|
MODULES += platforms/qurt/tests/hello
|
||||||
#MODULES += platforms/posix/tests/vcdev_test
|
MODULES += platforms/posix/tests/vcdev_test
|
||||||
#MODULES += platforms/posix/tests/hrt_test
|
#MODULES += platforms/posix/tests/hrt_test
|
||||||
#MODULES += platforms/posix/tests/wqueue
|
MODULES += platforms/posix/tests/wqueue
|
||||||
|
|
||||||
|
|
|
@ -108,6 +108,10 @@ I2C::init()
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef __PX4_QURT
|
||||||
|
simulate = true;
|
||||||
|
#endif
|
||||||
|
|
||||||
if (simulate) {
|
if (simulate) {
|
||||||
_fd = 10000;
|
_fd = 10000;
|
||||||
}
|
}
|
||||||
|
|
|
@ -48,8 +48,6 @@
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
#define PX4_DEBUG(...)
|
|
||||||
|
|
||||||
using namespace device;
|
using namespace device;
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
|
@ -36,7 +36,7 @@
|
||||||
* A device simulator
|
* A device simulator
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <px4_debug.h>
|
#include <px4_log.h>
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
#include <px4_time.h>
|
#include <px4_time.h>
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
* Implementation of existing task API for Linux
|
* Implementation of existing task API for Linux
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <px4_debug.h>
|
#include <px4_log.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
|
@ -57,14 +57,14 @@ int hello_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
printf("usage: hello {start|stop|status}\n");
|
PX4_WARN("usage: hello {start|stop|status}\n");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "start")) {
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
if (HelloExample::appState.isRunning()) {
|
if (HelloExample::appState.isRunning()) {
|
||||||
printf("already running\n");
|
PX4_INFO("already running\n");
|
||||||
/* this is not an error */
|
/* this is not an error */
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -86,15 +86,15 @@ int hello_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (HelloExample::appState.isRunning()) {
|
if (HelloExample::appState.isRunning()) {
|
||||||
printf("is running\n");
|
PX4_INFO("is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("not started\n");
|
PX4_INFO("not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("usage: hello_main {start|stop|status}\n");
|
PX4_WARN("usage: hello_main {start|stop|status}\n");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -59,7 +59,7 @@ static int writer_main(int argc, char *argv[])
|
||||||
|
|
||||||
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
|
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
printf("--- Open failed %d %d", fd, px4_errno);
|
PX4_INFO("--- Open failed %d %d", fd, px4_errno);
|
||||||
return -px4_errno;
|
return -px4_errno;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -67,14 +67,14 @@ static int writer_main(int argc, char *argv[])
|
||||||
int i=0;
|
int i=0;
|
||||||
while (i<3) {
|
while (i<3) {
|
||||||
// Wait for 3 seconds
|
// Wait for 3 seconds
|
||||||
printf("--- Sleeping for 4 sec\n");
|
PX4_INFO("--- Sleeping for 4 sec\n");
|
||||||
ret = sleep(4);
|
ret = sleep(4);
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
printf("--- sleep failed %d %d\n", ret, errno);
|
PX4_INFO("--- sleep failed %d %d\n", ret, errno);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("--- writing to fd\n");
|
PX4_INFO("--- writing to fd\n");
|
||||||
ret = px4_write(fd, buf, 1);
|
ret = px4_write(fd, buf, 1);
|
||||||
++i;
|
++i;
|
||||||
}
|
}
|
||||||
|
@ -110,16 +110,16 @@ 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) {
|
||||||
printf("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) {
|
||||||
printf("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;
|
||||||
}
|
}
|
||||||
printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
|
PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -131,29 +131,29 @@ int VCDevExample::main()
|
||||||
_node = new VCDevNode();
|
_node = new VCDevNode();
|
||||||
|
|
||||||
if (_node == 0) {
|
if (_node == 0) {
|
||||||
printf("Failed to allocate VCDevNode\n");
|
PX4_INFO("Failed to allocate VCDevNode\n");
|
||||||
return -ENOMEM;
|
return -ENOMEM;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_node->init() != PX4_OK) {
|
if (_node->init() != PX4_OK) {
|
||||||
printf("Failed to init VCDevNode\n");
|
PX4_INFO("Failed to init VCDevNode\n");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
|
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
|
||||||
|
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
printf("Open failed %d %d", fd, px4_errno);
|
PX4_INFO("Open failed %d %d", fd, px4_errno);
|
||||||
return -px4_errno;
|
return -px4_errno;
|
||||||
}
|
}
|
||||||
|
|
||||||
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) {
|
||||||
printf("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;
|
||||||
}
|
}
|
||||||
printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
|
PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
|
||||||
|
|
||||||
ret = test_pub_block(fd, 1);
|
ret = test_pub_block(fd, 1);
|
||||||
if (ret < 0)
|
if (ret < 0)
|
||||||
|
@ -174,32 +174,32 @@ int VCDevExample::main()
|
||||||
(char* const*)NULL);
|
(char* const*)NULL);
|
||||||
|
|
||||||
while (!appState.exitRequested() && i<13) {
|
while (!appState.exitRequested() && i<13) {
|
||||||
printf("=====================\n");
|
PX4_INFO("=====================\n");
|
||||||
printf("==== sleeping 2 sec ====\n");
|
PX4_INFO("==== sleeping 2 sec ====\n");
|
||||||
sleep(2);
|
sleep(2);
|
||||||
|
|
||||||
fds[0].fd = fd;
|
fds[0].fd = fd;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
fds[0].revents = 0;
|
fds[0].revents = 0;
|
||||||
|
|
||||||
printf("==== Calling Poll\n");
|
PX4_INFO("==== Calling Poll\n");
|
||||||
ret = px4_poll(fds, 1, 1000);
|
ret = px4_poll(fds, 1, 1000);
|
||||||
printf("==== Done poll\n");
|
PX4_INFO("==== Done poll\n");
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
printf("==== poll failed %d %d\n", ret, px4_errno);
|
PX4_INFO("==== poll failed %d %d\n", ret, px4_errno);
|
||||||
px4_close(fd);
|
px4_close(fd);
|
||||||
}
|
}
|
||||||
else if (i > 0) {
|
else if (i > 0) {
|
||||||
if (ret == 0)
|
if (ret == 0)
|
||||||
printf("==== Nothing to read - PASS\n");
|
PX4_INFO("==== Nothing to read - PASS\n");
|
||||||
else
|
else
|
||||||
printf("==== poll returned %d\n", ret);
|
PX4_INFO("==== poll returned %d\n", ret);
|
||||||
}
|
}
|
||||||
else if (i == 0) {
|
else if (i == 0) {
|
||||||
if (ret == 1)
|
if (ret == 1)
|
||||||
printf("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
|
PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
|
||||||
else
|
else
|
||||||
printf("==== %d to read - FAIL\n", ret);
|
PX4_INFO("==== %d to read - FAIL\n", ret);
|
||||||
|
|
||||||
}
|
}
|
||||||
++i;
|
++i;
|
||||||
|
|
|
@ -37,6 +37,7 @@
|
||||||
*
|
*
|
||||||
* @author Mark Charlebois <charlebm@gmail.com>
|
* @author Mark Charlebois <charlebm@gmail.com>
|
||||||
*/
|
*/
|
||||||
|
#include <px4_log.h>
|
||||||
#include <px4_middleware.h>
|
#include <px4_middleware.h>
|
||||||
#include <px4_app.h>
|
#include <px4_app.h>
|
||||||
#include "wqueue_test.h"
|
#include "wqueue_test.h"
|
||||||
|
@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
|
||||||
{
|
{
|
||||||
px4::init(argc, argv, "wqueue_test");
|
px4::init(argc, argv, "wqueue_test");
|
||||||
|
|
||||||
printf("wqueue hello\n");
|
PX4_INFO("wqueue hello\n");
|
||||||
WQueueTest wq;
|
WQueueTest wq;
|
||||||
wq.main();
|
wq.main();
|
||||||
|
|
||||||
printf("goodbye\n");
|
PX4_INFO("goodbye\n");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -39,6 +39,7 @@
|
||||||
*/
|
*/
|
||||||
#include "wqueue_test.h"
|
#include "wqueue_test.h"
|
||||||
#include <px4_app.h>
|
#include <px4_app.h>
|
||||||
|
#include <px4_log.h>
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
@ -53,14 +54,14 @@ int wqueue_test_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
|
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
printf("usage: wqueue_test {start|stop|status}\n");
|
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!strcmp(argv[1], "start")) {
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
if (WQueueTest::appState.isRunning()) {
|
if (WQueueTest::appState.isRunning()) {
|
||||||
printf("already running\n");
|
PX4_INFO("already running\n");
|
||||||
/* this is not an error */
|
/* this is not an error */
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -82,15 +83,15 @@ int wqueue_test_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (WQueueTest::appState.isRunning()) {
|
if (WQueueTest::appState.isRunning()) {
|
||||||
printf("is running\n");
|
PX4_INFO("is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("not started\n");
|
PX4_INFO("not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("usage: wqueue_test {start|stop|status}\n");
|
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -39,7 +39,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <px4_debug.h>
|
#include <px4_log.h>
|
||||||
|
|
||||||
/* Get the name of the default value fiven the param name */
|
/* Get the name of the default value fiven the param name */
|
||||||
#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
|
#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
|
||||||
|
|
|
@ -32,13 +32,25 @@
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file px4_debug.h
|
* @file px4_log.h
|
||||||
* Platform dependant debug
|
* Platform dependant logging/debug
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#if defined(__PX4_LINUX) || defined(__PX4_QURT)
|
#if defined(__PX4_QURT)
|
||||||
|
|
||||||
|
__BEGIN_DECLS
|
||||||
|
extern void qurt_log(const char *fmt, ...);
|
||||||
|
__END_DECLS
|
||||||
|
|
||||||
|
#define PX4_DBG(...) qurt_log(__VA_ARGS__)
|
||||||
|
#define PX4_DEBUG(...) qurt_log(__VA_ARGS__)
|
||||||
|
#define PX4_INFO(...) qurt_log(__VA_ARGS__)
|
||||||
|
#define PX4_WARN(...) qurt_log(__VA_ARGS__)
|
||||||
|
#define PX4_ERR(...) { qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); }
|
||||||
|
|
||||||
|
#elif defined(__PX4_LINUX)
|
||||||
|
|
||||||
#if defined(__PX4_LINUX)
|
#if defined(__PX4_LINUX)
|
||||||
#include <err.h>
|
#include <err.h>
|
|
@ -10,10 +10,13 @@
|
||||||
|
|
||||||
#elif defined(__PX4_QURT)
|
#elif defined(__PX4_QURT)
|
||||||
|
|
||||||
|
#include <sys/timespec.h>
|
||||||
|
|
||||||
#define CLOCK_REALTIME 1
|
#define CLOCK_REALTIME 1
|
||||||
|
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
|
|
||||||
|
#if 0
|
||||||
#if !defined(__cplusplus)
|
#if !defined(__cplusplus)
|
||||||
struct timespec
|
struct timespec
|
||||||
{
|
{
|
||||||
|
@ -21,6 +24,7 @@ struct timespec
|
||||||
long tv_nsec;
|
long tv_nsec;
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
|
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
|
||||||
int px4_clock_settime(clockid_t clk_id, struct timespec *tp);
|
int px4_clock_settime(clockid_t clk_id, struct timespec *tp);
|
||||||
|
|
||||||
|
|
|
@ -52,6 +52,9 @@
|
||||||
#include "px4_middleware.h"
|
#include "px4_middleware.h"
|
||||||
|
|
||||||
static const char *commands =
|
static const char *commands =
|
||||||
|
"x\n"
|
||||||
|
"hello start"
|
||||||
|
#if 0
|
||||||
"uorb start\n"
|
"uorb start\n"
|
||||||
"simulator start -s\n"
|
"simulator start -s\n"
|
||||||
"barosim start\n"
|
"barosim start\n"
|
||||||
|
@ -68,7 +71,9 @@ static const char *commands =
|
||||||
"hil mode_pwm\n"
|
"hil mode_pwm\n"
|
||||||
"commander start\n"
|
"commander start\n"
|
||||||
"list_devices\n"
|
"list_devices\n"
|
||||||
"list_topics\n";
|
"list_topics\n"
|
||||||
|
#endif
|
||||||
|
;
|
||||||
|
|
||||||
|
|
||||||
static void run_cmd(const vector<string> &appargs) {
|
static void run_cmd(const vector<string> &appargs) {
|
||||||
|
@ -148,8 +153,13 @@ static void process_commands(const char *cmds)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace px4 {
|
||||||
|
extern void init_once(void);
|
||||||
|
};
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
|
px4::init_once();
|
||||||
px4::init(argc, argv, "mainapp");
|
px4::init(argc, argv, "mainapp");
|
||||||
process_commands(commands);
|
process_commands(commands);
|
||||||
for (;;) {}
|
for (;;) {}
|
||||||
|
|
|
@ -37,9 +37,12 @@
|
||||||
* High-resolution timer with callouts and timekeeping.
|
* High-resolution timer with callouts and timekeeping.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <px4_workqueue.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <semaphore.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
static struct sq_queue_s callout_queue;
|
static struct sq_queue_s callout_queue;
|
||||||
|
|
||||||
|
@ -51,9 +54,28 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
|
||||||
|
|
||||||
static void hrt_call_reschedule(void);
|
static void hrt_call_reschedule(void);
|
||||||
|
|
||||||
|
// Intervals in ms
|
||||||
#define HRT_INTERVAL_MIN 50
|
#define HRT_INTERVAL_MIN 50
|
||||||
#define HRT_INTERVAL_MAX 50000
|
#define HRT_INTERVAL_MAX 50000
|
||||||
|
|
||||||
|
static sem_t _hrt_lock;
|
||||||
|
static struct work_s _hrt_work;
|
||||||
|
|
||||||
|
static void
|
||||||
|
hrt_call_invoke(void);
|
||||||
|
|
||||||
|
static void hrt_lock(void)
|
||||||
|
{
|
||||||
|
//printf("hrt_lock\n");
|
||||||
|
sem_wait(&_hrt_lock);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void hrt_unlock(void)
|
||||||
|
{
|
||||||
|
//printf("hrt_unlock\n");
|
||||||
|
sem_post(&_hrt_lock);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Get absolute time.
|
* Get absolute time.
|
||||||
*/
|
*/
|
||||||
|
@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void)
|
||||||
{
|
{
|
||||||
struct timespec ts;
|
struct timespec ts;
|
||||||
|
|
||||||
// FIXME - not supported in QURT
|
// FIXME - clock_gettime unsupported in QuRT
|
||||||
//clock_gettime(CLOCK_MONOTONIC, &ts);
|
//clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
return ts_to_abstime(&ts);
|
return ts_to_abstime(&ts);
|
||||||
}
|
}
|
||||||
|
@ -120,7 +142,7 @@ bool hrt_called(struct hrt_call *entry)
|
||||||
*/
|
*/
|
||||||
void hrt_cancel(struct hrt_call *entry)
|
void hrt_cancel(struct hrt_call *entry)
|
||||||
{
|
{
|
||||||
// FIXME - need a lock
|
hrt_lock();
|
||||||
sq_rem(&entry->link, &callout_queue);
|
sq_rem(&entry->link, &callout_queue);
|
||||||
entry->deadline = 0;
|
entry->deadline = 0;
|
||||||
|
|
||||||
|
@ -128,6 +150,7 @@ void hrt_cancel(struct hrt_call *entry)
|
||||||
* being re-entered when the callout returns.
|
* being re-entered when the callout returns.
|
||||||
*/
|
*/
|
||||||
entry->period = 0;
|
entry->period = 0;
|
||||||
|
hrt_unlock();
|
||||||
// endif
|
// endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -156,7 +179,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
|
||||||
*/
|
*/
|
||||||
void hrt_init(void)
|
void hrt_init(void)
|
||||||
{
|
{
|
||||||
|
//printf("hrt_init\n");
|
||||||
sq_init(&callout_queue);
|
sq_init(&callout_queue);
|
||||||
|
sem_init(&_hrt_lock, 0, 1);
|
||||||
|
memset(&_hrt_work, 0, sizeof(_hrt_work));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
static void
|
||||||
|
@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry)
|
||||||
{
|
{
|
||||||
struct hrt_call *call, *next;
|
struct hrt_call *call, *next;
|
||||||
|
|
||||||
|
//printf("hrt_call_enter\n");
|
||||||
call = (struct hrt_call *)sq_peek(&callout_queue);
|
call = (struct hrt_call *)sq_peek(&callout_queue);
|
||||||
|
|
||||||
if ((call == NULL) || (entry->deadline < call->deadline)) {
|
if ((call == NULL) || (entry->deadline < call->deadline)) {
|
||||||
|
@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry)
|
||||||
//lldbg("scheduled\n");
|
//lldbg("scheduled\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Timer interrupt handler
|
||||||
|
*
|
||||||
|
* This routine simulates a timer interrupt handler
|
||||||
|
*/
|
||||||
|
static void
|
||||||
|
hrt_tim_isr(void *p)
|
||||||
|
{
|
||||||
|
|
||||||
|
//printf("hrt_tim_isr\n");
|
||||||
|
/* run any callouts that have met their deadline */
|
||||||
|
hrt_call_invoke();
|
||||||
|
|
||||||
|
hrt_lock();
|
||||||
|
|
||||||
|
/* and schedule the next interrupt */
|
||||||
|
hrt_call_reschedule();
|
||||||
|
|
||||||
|
hrt_unlock();
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Reschedule the next timer interrupt.
|
* Reschedule the next timer interrupt.
|
||||||
*
|
*
|
||||||
|
@ -198,7 +246,10 @@ hrt_call_reschedule()
|
||||||
hrt_abstime now = hrt_absolute_time();
|
hrt_abstime now = hrt_absolute_time();
|
||||||
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
|
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
|
||||||
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
|
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
|
||||||
|
uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
|
||||||
|
|
||||||
|
//printf("hrt_call_reschedule\n");
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Determine what the next deadline will be.
|
* Determine what the next deadline will be.
|
||||||
*
|
*
|
||||||
|
@ -216,18 +267,26 @@ hrt_call_reschedule()
|
||||||
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
|
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
|
||||||
//lldbg("pre-expired\n");
|
//lldbg("pre-expired\n");
|
||||||
/* set a minimal deadline so that we call ASAP */
|
/* set a minimal deadline so that we call ASAP */
|
||||||
deadline = now + HRT_INTERVAL_MIN;
|
ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
|
||||||
|
|
||||||
} else if (next->deadline < deadline) {
|
} else if (next->deadline < deadline) {
|
||||||
//lldbg("due soon\n");
|
//lldbg("due soon\n");
|
||||||
deadline = next->deadline;
|
ticks = USEC2TICK((next->deadline - now)*1000);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// There is no timer ISR, so simulate one by putting an event on the
|
||||||
|
// high priority work queue
|
||||||
|
//printf("ticks = %u\n", ticks);
|
||||||
|
work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
static void
|
||||||
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
|
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
|
||||||
{
|
{
|
||||||
|
//printf("hrt_call_internal\n");
|
||||||
|
hrt_lock();
|
||||||
|
//printf("hrt_call_internal after lock\n");
|
||||||
/* if the entry is currently queued, remove it */
|
/* if the entry is currently queued, remove it */
|
||||||
/* note that we are using a potentially uninitialised
|
/* note that we are using a potentially uninitialised
|
||||||
entry->link here, but it is safe as sq_rem() doesn't
|
entry->link here, but it is safe as sq_rem() doesn't
|
||||||
|
@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
||||||
entry->arg = arg;
|
entry->arg = arg;
|
||||||
|
|
||||||
hrt_call_enter(entry);
|
hrt_call_enter(entry);
|
||||||
|
hrt_unlock();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -255,6 +315,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
|
||||||
*/
|
*/
|
||||||
void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
||||||
{
|
{
|
||||||
|
//printf("hrt_call_after\n");
|
||||||
hrt_call_internal(entry,
|
hrt_call_internal(entry,
|
||||||
hrt_absolute_time() + delay,
|
hrt_absolute_time() + delay,
|
||||||
0,
|
0,
|
||||||
|
@ -292,3 +353,57 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo
|
||||||
void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
|
void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
static void
|
||||||
|
hrt_call_invoke(void)
|
||||||
|
{
|
||||||
|
struct hrt_call *call;
|
||||||
|
hrt_abstime deadline;
|
||||||
|
|
||||||
|
hrt_lock();
|
||||||
|
while (true) {
|
||||||
|
/* get the current time */
|
||||||
|
hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
|
call = (struct hrt_call *)sq_peek(&callout_queue);
|
||||||
|
|
||||||
|
if (call == NULL)
|
||||||
|
break;
|
||||||
|
|
||||||
|
if (call->deadline > now)
|
||||||
|
break;
|
||||||
|
|
||||||
|
sq_rem(&call->link, &callout_queue);
|
||||||
|
//lldbg("call pop\n");
|
||||||
|
|
||||||
|
/* save the intended deadline for periodic calls */
|
||||||
|
deadline = call->deadline;
|
||||||
|
|
||||||
|
/* zero the deadline, as the call has occurred */
|
||||||
|
call->deadline = 0;
|
||||||
|
|
||||||
|
/* invoke the callout (if there is one) */
|
||||||
|
if (call->callout) {
|
||||||
|
// Unlock so we don't deadlock in callback
|
||||||
|
hrt_unlock();
|
||||||
|
|
||||||
|
//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
|
||||||
|
call->callout(call->arg);
|
||||||
|
|
||||||
|
hrt_lock();
|
||||||
|
}
|
||||||
|
|
||||||
|
/* if the callout has a non-zero period, it has to be re-entered */
|
||||||
|
if (call->period != 0) {
|
||||||
|
// re-check call->deadline to allow for
|
||||||
|
// callouts to re-schedule themselves
|
||||||
|
// using hrt_call_delay()
|
||||||
|
if (call->deadline <= now) {
|
||||||
|
call->deadline = deadline + call->period;
|
||||||
|
}
|
||||||
|
|
||||||
|
hrt_call_enter(call);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
hrt_unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000;
|
||||||
void usleep(useconds_t usec) {}
|
void usleep(useconds_t usec) {}
|
||||||
unsigned int sleep(unsigned int sec) { return 0; }
|
unsigned int sleep(unsigned int sec) { return 0; }
|
||||||
|
|
||||||
|
extern void hrt_init(void);
|
||||||
|
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|
||||||
extern struct wqueue_s gwork[NWORKERS];
|
extern struct wqueue_s gwork[NWORKERS];
|
||||||
|
@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS];
|
||||||
namespace px4
|
namespace px4
|
||||||
{
|
{
|
||||||
|
|
||||||
|
void init_once(void);
|
||||||
|
|
||||||
|
void init_once(void)
|
||||||
|
{
|
||||||
|
work_queues_init();
|
||||||
|
hrt_init();
|
||||||
|
}
|
||||||
|
|
||||||
void init(int argc, char *argv[], const char *app_name)
|
void init(int argc, char *argv[], const char *app_name)
|
||||||
{
|
{
|
||||||
printf("App name: %s\n", app_name);
|
printf("App name: %s\n", app_name);
|
||||||
|
|
|
@ -33,11 +33,10 @@
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file px4_qurt_tasks.c
|
* @file px4_linux_tasks.c
|
||||||
* Implementation of existing task API for Linux
|
* Implementation of existing task API for Linux
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <px4_debug.h>
|
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
@ -54,17 +53,20 @@
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
|
#include <px4_log.h>
|
||||||
#include <hexagon_standalone.h>
|
#include <hexagon_standalone.h>
|
||||||
|
|
||||||
#define MAX_CMD_LEN 100
|
#define MAX_CMD_LEN 100
|
||||||
|
|
||||||
#define PX4_MAX_TASKS 100
|
#define PX4_MAX_TASKS 5
|
||||||
|
|
||||||
struct task_entry
|
struct task_entry
|
||||||
{
|
{
|
||||||
pthread_t pid;
|
int pid;
|
||||||
std::string name;
|
std::string name;
|
||||||
bool isused;
|
bool isused;
|
||||||
task_entry() : isused(false) {}
|
task_entry() : isused(false) {}
|
||||||
|
void *sp;
|
||||||
};
|
};
|
||||||
|
|
||||||
static task_entry taskmap[PX4_MAX_TASKS];
|
static task_entry taskmap[PX4_MAX_TASKS];
|
||||||
|
@ -73,32 +75,31 @@ typedef struct
|
||||||
{
|
{
|
||||||
px4_main_t entry;
|
px4_main_t entry;
|
||||||
int argc;
|
int argc;
|
||||||
char *argv[];
|
char * argv[];
|
||||||
// strings are allocated after the
|
// strings are allocated after
|
||||||
} pthdata_t;
|
} pthdata_t;
|
||||||
|
|
||||||
static void *entry_adapter ( void *ptr )
|
static void entry_adapter ( void *ptr )
|
||||||
{
|
{
|
||||||
PX4_DBG("entry_adapter\n");
|
pthdata_t *data = (pthdata_t *) ptr;
|
||||||
pthdata_t *data;
|
PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]);
|
||||||
data = (pthdata_t *) ptr;
|
|
||||||
|
|
||||||
|
PX4_DEBUG("data->entry = %p\n", data->entry);
|
||||||
data->entry(data->argc, data->argv);
|
data->entry(data->argc, data->argv);
|
||||||
free(ptr);
|
free(ptr);
|
||||||
PX4_DBG("Before px4_task_exit");
|
PX4_DEBUG("after entry\n");
|
||||||
|
PX4_DEBUG("Before px4_task_exit\n");
|
||||||
px4_task_exit(0);
|
px4_task_exit(0);
|
||||||
PX4_DBG("After px4_task_exit");
|
PX4_DEBUG("After px4_task_exit\n");
|
||||||
|
|
||||||
return NULL;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
px4_systemreset(bool to_bootloader)
|
px4_systemreset(bool to_bootloader)
|
||||||
{
|
{
|
||||||
PX4_WARN("Called px4_system_reset");
|
PX4_DEBUG("Called px4_system_reset\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
|
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv)
|
||||||
{
|
{
|
||||||
int rv;
|
int rv;
|
||||||
int argc = 0;
|
int argc = 0;
|
||||||
|
@ -108,10 +109,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
|
||||||
unsigned long structsize;
|
unsigned long structsize;
|
||||||
char * p = (char *)argv;
|
char * p = (char *)argv;
|
||||||
|
|
||||||
pthread_t task;
|
PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]);
|
||||||
pthread_attr_t attr;
|
|
||||||
struct sched_param param;
|
|
||||||
|
|
||||||
// Calculate argc
|
// Calculate argc
|
||||||
while (p != (char *)0) {
|
while (p != (char *)0) {
|
||||||
p = argv[argc];
|
p = argv[argc];
|
||||||
|
@ -120,18 +118,24 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
|
||||||
++argc;
|
++argc;
|
||||||
len += strlen(p)+1;
|
len += strlen(p)+1;
|
||||||
}
|
}
|
||||||
|
PX4_DEBUG("arg %d %p\n", argc, argv);
|
||||||
structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
|
structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
|
||||||
pthdata_t *taskdata;
|
pthdata_t *taskdata;
|
||||||
|
|
||||||
|
PX4_DEBUG("arg %d %p\n", argc, argv);
|
||||||
// not safe to pass stack data to the thread creation
|
// not safe to pass stack data to the thread creation
|
||||||
taskdata = (pthdata_t *)malloc(structsize+len);
|
taskdata = (pthdata_t *)malloc(structsize+len);
|
||||||
|
PX4_DEBUG("arg %d %p\n", argc, argv);
|
||||||
offset = ((unsigned long)taskdata)+structsize;
|
offset = ((unsigned long)taskdata)+structsize;
|
||||||
|
PX4_DEBUG("arg %d %p\n", argc, argv);
|
||||||
|
|
||||||
taskdata->entry = entry;
|
taskdata->entry = entry;
|
||||||
taskdata->argc = argc;
|
taskdata->argc = argc;
|
||||||
|
|
||||||
|
PX4_DEBUG("arg %d %p\n", argc, argv);
|
||||||
for (i=0; i<argc; i++) {
|
for (i=0; i<argc; i++) {
|
||||||
printf("arg %d %s\n", i, argv[i]);
|
PX4_DEBUG("TEST\n");
|
||||||
|
PX4_DEBUG("arg %d %s\n", i, argv[i]);
|
||||||
taskdata->argv[i] = (char *)offset;
|
taskdata->argv[i] = (char *)offset;
|
||||||
strcpy((char *)offset, argv[i]);
|
strcpy((char *)offset, argv[i]);
|
||||||
offset+=strlen(argv[i])+1;
|
offset+=strlen(argv[i])+1;
|
||||||
|
@ -139,124 +143,38 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
|
||||||
// Must add NULL at end of argv
|
// Must add NULL at end of argv
|
||||||
taskdata->argv[argc] = (char *)0;
|
taskdata->argv[argc] = (char *)0;
|
||||||
|
|
||||||
rv = pthread_attr_init(&attr);
|
|
||||||
if (rv != 0) {
|
|
||||||
PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
#if 0
|
|
||||||
rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
|
|
||||||
if (rv != 0) {
|
|
||||||
PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
rv = pthread_attr_setschedpolicy(&attr, scheduler);
|
|
||||||
if (rv != 0) {
|
|
||||||
PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
|
|
||||||
param.sched_priority = priority;
|
|
||||||
|
|
||||||
rv = pthread_attr_setschedparam(&attr, ¶m);
|
|
||||||
if (rv != 0) {
|
|
||||||
PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata);
|
|
||||||
if (rv != 0) {
|
|
||||||
|
|
||||||
if (rv == EPERM) {
|
|
||||||
//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
|
|
||||||
rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
|
|
||||||
if (rv != 0) {
|
|
||||||
PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return (rv < 0) ? rv : -rv;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for (i=0; i<PX4_MAX_TASKS; ++i) {
|
for (i=0; i<PX4_MAX_TASKS; ++i) {
|
||||||
if (taskmap[i].isused == false) {
|
if (taskmap[i].isused == false) {
|
||||||
taskmap[i].pid = task;
|
taskmap[i].pid = i+1;
|
||||||
taskmap[i].name = name;
|
taskmap[i].name = name;
|
||||||
taskmap[i].isused = true;
|
taskmap[i].isused = true;
|
||||||
//taskmap[i].sp = malloc(stack_size);
|
taskmap[i].sp = malloc(2048);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (i>=PX4_MAX_TASKS) {
|
PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]);
|
||||||
return -ENOSPC;
|
thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata);
|
||||||
}
|
|
||||||
return i;
|
return i+1;
|
||||||
}
|
}
|
||||||
|
|
||||||
int px4_task_delete(px4_task_t id)
|
int px4_task_delete(px4_task_t id)
|
||||||
{
|
{
|
||||||
int rv = 0;
|
PX4_DEBUG("Called px4_task_delete\n");
|
||||||
pthread_t pid;
|
return -EINVAL;
|
||||||
PX4_WARN("Called px4_task_delete");
|
|
||||||
|
|
||||||
if (id < PX4_MAX_TASKS && taskmap[id].isused)
|
|
||||||
pid = taskmap[id].pid;
|
|
||||||
else
|
|
||||||
return -EINVAL;
|
|
||||||
|
|
||||||
// If current thread then exit, otherwise cancel
|
|
||||||
if (pthread_self() == pid) {
|
|
||||||
taskmap[id].isused = false;
|
|
||||||
pthread_exit(0);
|
|
||||||
} else {
|
|
||||||
rv = pthread_cancel(pid);
|
|
||||||
}
|
|
||||||
|
|
||||||
taskmap[id].isused = false;
|
|
||||||
|
|
||||||
return rv;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void px4_task_exit(int ret)
|
void px4_task_exit(int ret)
|
||||||
{
|
{
|
||||||
int i;
|
thread_stop();
|
||||||
pthread_t pid = pthread_self();
|
|
||||||
|
|
||||||
// Get pthread ID from the opaque ID
|
// Free stack
|
||||||
for (i=0; i<PX4_MAX_TASKS; ++i) {
|
|
||||||
if (taskmap[i].pid == pid) {
|
|
||||||
taskmap[i].isused = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (i>=PX4_MAX_TASKS) {
|
|
||||||
PX4_ERR("px4_task_exit: self task not found!");
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str());
|
|
||||||
}
|
|
||||||
|
|
||||||
pthread_exit((void *)(unsigned long)ret);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int px4_task_kill(px4_task_t id, int sig)
|
int px4_task_kill(px4_task_t id, int sig)
|
||||||
{
|
{
|
||||||
int rv = 0;
|
PX4_DEBUG("Called px4_task_kill\n");
|
||||||
pthread_t pid;
|
return -EINVAL;
|
||||||
PX4_DBG("Called px4_task_kill %d", sig);
|
|
||||||
|
|
||||||
if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
|
|
||||||
pid = taskmap[id].pid;
|
|
||||||
else
|
|
||||||
return -EINVAL;
|
|
||||||
|
|
||||||
// If current thread then exit, otherwise cancel
|
|
||||||
rv = pthread_kill(pid, sig);
|
|
||||||
|
|
||||||
return rv;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void px4_show_tasks()
|
void px4_show_tasks()
|
||||||
|
@ -264,16 +182,17 @@ void px4_show_tasks()
|
||||||
int idx;
|
int idx;
|
||||||
int count = 0;
|
int count = 0;
|
||||||
|
|
||||||
PX4_INFO("Active Tasks:");
|
PX4_DEBUG("Active Tasks:\n");
|
||||||
for (idx=0; idx < PX4_MAX_TASKS; idx++)
|
for (idx=0; idx < PX4_MAX_TASKS; idx++)
|
||||||
{
|
{
|
||||||
if (taskmap[idx].isused) {
|
if (taskmap[idx].isused) {
|
||||||
PX4_INFO(" %-10s", taskmap[idx].name.c_str());
|
PX4_DEBUG(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid);
|
||||||
count++;
|
count++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (count == 0)
|
if (count == 0)
|
||||||
PX4_INFO(" No running tasks");
|
PX4_DEBUG(" No running tasks\n");
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// STUBS
|
// STUBS
|
||||||
|
@ -282,4 +201,7 @@ extern "C" {
|
||||||
void hrt_sleep(unsigned long)
|
void hrt_sleep(unsigned long)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
int ioctl(int d, int request, unsigned long foo) { return 0; }
|
||||||
|
int write(int a, char const*b, int c) { return c; }
|
||||||
|
|
|
@ -0,0 +1,53 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <semaphore.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
extern sem_t _work_lock[];
|
||||||
|
|
||||||
|
inline void work_lock(int id);
|
||||||
|
inline void work_lock(int id)
|
||||||
|
{
|
||||||
|
//printf("work_lock %d\n", id);
|
||||||
|
sem_wait(&_work_lock[id]);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void work_unlock(int id);
|
||||||
|
inline void work_unlock(int id)
|
||||||
|
{
|
||||||
|
//printf("work_unlock %d\n", id);
|
||||||
|
sem_post(&_work_lock[id]);
|
||||||
|
}
|
||||||
|
|
|
@ -43,7 +43,10 @@
|
||||||
#include <signal.h>
|
#include <signal.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <queue.h>
|
#include <queue.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <semaphore.h>
|
||||||
#include <px4_workqueue.h>
|
#include <px4_workqueue.h>
|
||||||
|
#include "work_lock.h"
|
||||||
|
|
||||||
#ifdef CONFIG_SCHED_WORKQUEUE
|
#ifdef CONFIG_SCHED_WORKQUEUE
|
||||||
|
|
||||||
|
@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
|
||||||
* from with task logic or interrupt handlers.
|
* from with task logic or interrupt handlers.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//flags = irqsave();
|
work_lock(qid);
|
||||||
work->qtime = clock_systimer(); /* Time work queued */
|
work->qtime = clock_systimer(); /* Time work queued */
|
||||||
|
|
||||||
dq_addlast((dq_entry_t *)work, &wqueue->q);
|
dq_addlast((dq_entry_t *)work, &wqueue->q);
|
||||||
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
|
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
|
||||||
|
|
||||||
//irqrestore(flags);
|
work_unlock(qid);
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -39,12 +39,14 @@
|
||||||
|
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_defines.h>
|
#include <px4_defines.h>
|
||||||
|
#include <px4_time.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <queue.h>
|
#include <queue.h>
|
||||||
#include <px4_workqueue.h>
|
#include <px4_workqueue.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include "work_lock.h"
|
||||||
|
|
||||||
#ifdef CONFIG_SCHED_WORKQUEUE
|
#ifdef CONFIG_SCHED_WORKQUEUE
|
||||||
|
|
||||||
|
@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS];
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Private Variables
|
* Private Variables
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
sem_t _work_lock[NWORKERS];
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Private Functions
|
* Private Functions
|
||||||
|
@ -85,13 +88,12 @@ struct wqueue_s g_work[NWORKERS];
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
static void work_process(FAR struct wqueue_s *wqueue)
|
static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
|
||||||
{
|
{
|
||||||
volatile FAR struct work_s *work;
|
volatile FAR struct work_s *work;
|
||||||
worker_t worker;
|
worker_t worker;
|
||||||
//irqstate_t flags;
|
|
||||||
FAR void *arg;
|
FAR void *arg;
|
||||||
uint32_t elapsed;
|
uint64_t elapsed;
|
||||||
uint32_t remaining;
|
uint32_t remaining;
|
||||||
uint32_t next;
|
uint32_t next;
|
||||||
|
|
||||||
|
@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
next = CONFIG_SCHED_WORKPERIOD;
|
next = CONFIG_SCHED_WORKPERIOD;
|
||||||
//flags = irqsave();
|
|
||||||
|
work_lock(lock_id);
|
||||||
|
|
||||||
work = (FAR struct work_s *)wqueue->q.head;
|
work = (FAR struct work_s *)wqueue->q.head;
|
||||||
while (work)
|
while (work)
|
||||||
{
|
{
|
||||||
|
@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
* zero. Therefore a delay of zero will always execute immediately.
|
* zero. Therefore a delay of zero will always execute immediately.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
elapsed = clock_systimer() - work->qtime;
|
elapsed = USEC2TICK(clock_systimer() - work->qtime);
|
||||||
//printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
|
//printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
|
||||||
if (elapsed >= work->delay)
|
if (elapsed >= work->delay)
|
||||||
{
|
{
|
||||||
/* Remove the ready-to-execute work from the list */
|
/* Remove the ready-to-execute work from the list */
|
||||||
|
@ -133,9 +137,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
* performed... we don't have any idea how long that will take!
|
* performed... we don't have any idea how long that will take!
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//irqrestore(flags);
|
work_unlock(lock_id);
|
||||||
if (!worker) {
|
if (!worker) {
|
||||||
printf("MESSED UP: worker = 0\n");
|
PX4_WARN("MESSED UP: worker = 0\n");
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
worker(arg);
|
worker(arg);
|
||||||
|
@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
* back at the head of the list.
|
* back at the head of the list.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
//flags = irqsave();
|
work_lock(lock_id);
|
||||||
work = (FAR struct work_s *)wqueue->q.head;
|
work = (FAR struct work_s *)wqueue->q.head;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Here: elapsed < work->delay */
|
/* Here: elapsed < work->delay */
|
||||||
remaining = work->delay - elapsed;
|
remaining = USEC_PER_TICK*(work->delay - elapsed);
|
||||||
if (remaining < next)
|
if (remaining < next)
|
||||||
{
|
{
|
||||||
/* Yes.. Then schedule to wake up when the work is ready */
|
/* Yes.. Then schedule to wake up when the work is ready */
|
||||||
|
@ -172,15 +176,40 @@ static void work_process(FAR struct wqueue_s *wqueue)
|
||||||
/* Wait awhile to check the work list. We will wait here until either
|
/* Wait awhile to check the work list. We will wait here until either
|
||||||
* the time elapses or until we are awakened by a signal.
|
* the time elapses or until we are awakened by a signal.
|
||||||
*/
|
*/
|
||||||
|
work_unlock(lock_id);
|
||||||
|
|
||||||
//FIXME - DSPAL doesn't support usleep
|
usleep(next);
|
||||||
//usleep(next);
|
|
||||||
//irqrestore(flags);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Public Functions
|
* Public Functions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
void work_queues_init(void)
|
||||||
|
{
|
||||||
|
sem_init(&_work_lock[HPWORK], 0, 1);
|
||||||
|
sem_init(&_work_lock[LPWORK], 0, 1);
|
||||||
|
#ifdef CONFIG_SCHED_USRWORK
|
||||||
|
sem_init(&_work_lock[USRWORK], 0, 1);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Create high priority worker thread
|
||||||
|
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MAX-1,
|
||||||
|
2000,
|
||||||
|
work_hpthread,
|
||||||
|
(char* const*)NULL);
|
||||||
|
|
||||||
|
// Create low priority worker thread
|
||||||
|
g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MIN,
|
||||||
|
2000,
|
||||||
|
work_lpthread,
|
||||||
|
(char* const*)NULL);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Name: work_hpthread, work_lpthread, and work_usrthread
|
* Name: work_hpthread, work_lpthread, and work_usrthread
|
||||||
*
|
*
|
||||||
|
@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[])
|
||||||
* we process items in the work list.
|
* we process items in the work list.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
work_process(&g_work[HPWORK]);
|
work_process(&g_work[HPWORK], HPWORK);
|
||||||
}
|
}
|
||||||
|
|
||||||
return PX4_OK; /* To keep some compilers happy */
|
return PX4_OK; /* To keep some compilers happy */
|
||||||
|
@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[])
|
||||||
* we process items in the work list.
|
* we process items in the work list.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
work_process(&g_work[LPWORK]);
|
work_process(&g_work[LPWORK], LPWORK);
|
||||||
}
|
}
|
||||||
|
|
||||||
return PX4_OK; /* To keep some compilers happy */
|
return PX4_OK; /* To keep some compilers happy */
|
||||||
|
@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[])
|
||||||
* we process items in the work list.
|
* we process items in the work list.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
work_process(&g_work[USRWORK]);
|
work_process(&g_work[USRWORK], USRWORK);
|
||||||
}
|
}
|
||||||
|
|
||||||
return PX4_OK; /* To keep some compilers happy */
|
return PX4_OK; /* To keep some compilers happy */
|
||||||
|
@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[])
|
||||||
|
|
||||||
uint32_t clock_systimer()
|
uint32_t clock_systimer()
|
||||||
{
|
{
|
||||||
|
//printf("clock_systimer: %0lx\n", hrt_absolute_time());
|
||||||
return (0x00000000ffffffff & hrt_absolute_time());
|
return (0x00000000ffffffff & hrt_absolute_time());
|
||||||
}
|
}
|
||||||
#endif /* CONFIG_SCHED_WORKQUEUE */
|
#endif /* CONFIG_SCHED_WORKQUEUE */
|
||||||
|
|
|
@ -1,4 +1,3 @@
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
|
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
|
||||||
|
@ -40,6 +39,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "hello_example.h"
|
#include "hello_example.h"
|
||||||
|
#include <px4_log.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
|
@ -52,7 +52,7 @@ int HelloExample::main()
|
||||||
int i=0;
|
int i=0;
|
||||||
while (!appState.exitRequested() && i<5) {
|
while (!appState.exitRequested() && i<5) {
|
||||||
|
|
||||||
printf(" Doing work...\n");
|
PX4_DEBUG(" Doing work...\n");
|
||||||
++i;
|
++i;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -38,6 +38,7 @@
|
||||||
* @author Mark Charlebois <charlebm@gmail.com>
|
* @author Mark Charlebois <charlebm@gmail.com>
|
||||||
*/
|
*/
|
||||||
#include <px4_middleware.h>
|
#include <px4_middleware.h>
|
||||||
|
#include <px4_log.h>
|
||||||
#include <px4_app.h>
|
#include <px4_app.h>
|
||||||
#include "hello_example.h"
|
#include "hello_example.h"
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
|
||||||
{
|
{
|
||||||
px4::init(argc, argv, "hello");
|
px4::init(argc, argv, "hello");
|
||||||
|
|
||||||
printf("hello\n");
|
PX4_DEBUG("hello\n");
|
||||||
HelloExample hello;
|
HelloExample hello;
|
||||||
hello.main();
|
hello.main();
|
||||||
|
|
||||||
printf("goodbye\n");
|
PX4_DEBUG("goodbye\n");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,6 +38,7 @@
|
||||||
* @author Mark Charlebois <mcharleb@gmail.com>
|
* @author Mark Charlebois <mcharleb@gmail.com>
|
||||||
*/
|
*/
|
||||||
#include "hello_example.h"
|
#include "hello_example.h"
|
||||||
|
#include <px4_log.h>
|
||||||
#include <px4_app.h>
|
#include <px4_app.h>
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
@ -52,11 +53,11 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]);
|
||||||
|
|
||||||
static void usage()
|
static void usage()
|
||||||
{
|
{
|
||||||
printf("usage: hello {start|stop|status}\n");
|
PX4_DEBUG("usage: hello {start|stop|status}\n");
|
||||||
}
|
}
|
||||||
int hello_main(int argc, char *argv[])
|
int hello_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
printf("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]);
|
PX4_DEBUG("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]);
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
usage();
|
usage();
|
||||||
return 1;
|
return 1;
|
||||||
|
@ -64,8 +65,9 @@ int hello_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!strcmp(argv[1], "start")) {
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
|
PX4_DEBUG("Starting\n");
|
||||||
if (HelloExample::appState.isRunning()) {
|
if (HelloExample::appState.isRunning()) {
|
||||||
printf("already running\n");
|
PX4_DEBUG("already running\n");
|
||||||
/* this is not an error */
|
/* this is not an error */
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -73,7 +75,7 @@ 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,
|
16000,
|
||||||
PX4_MAIN,
|
PX4_MAIN,
|
||||||
(char* const*)argv);
|
(char* const*)argv);
|
||||||
|
|
||||||
|
@ -87,10 +89,10 @@ int hello_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (HelloExample::appState.isRunning()) {
|
if (HelloExample::appState.isRunning()) {
|
||||||
printf("is running\n");
|
PX4_DEBUG("is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("not started\n");
|
PX4_DEBUG("not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
Loading…
Reference in New Issue