dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'qurt' into linux

This commit is contained in:
Mark Charlebois 2015-05-11 16:07:53 -07:00
parent 11010a648c a99f916bdf
commit 2d32395bc0
22 changed files with 364 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
makefiles/qurt/config_qurt_default.mk
View File

@ -72,7 +72,7 @@ MODULES += platforms/posix/drivers/barosim
# Unit tests
#
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/wqueue
MODULES += platforms/posix/tests/wqueue

4
src/drivers/device/i2c_posix.cpp
View File

@ -108,6 +108,10 @@ I2C::init()
return PX4_ERROR;
}
#ifdef __PX4_QURT
simulate = true;
#endif
if (simulate) {
_fd = 10000;
}

2
src/drivers/device/vdev_posix.cpp
View File

@ -48,8 +48,6 @@
#include <pthread.h>
#include <unistd.h>
#define PX4_DEBUG(...)
using namespace device;
extern "C" {

2
src/modules/simulator/simulator.cpp
View File

@ -36,7 +36,7 @@
* A device simulator
*/
#include <px4_debug.h>
#include <px4_log.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <pthread.h>

2
src/platforms/posix/px4_layer/px4_posix_tasks.cpp
View File

@ -37,7 +37,7 @@
* Implementation of existing task API for Linux
*/
#include <px4_debug.h>
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

10
src/platforms/posix/tests/hello/hello_start_posix.cpp
View File

@ -57,14 +57,14 @@ int hello_main(int argc, char *argv[])
{
if (argc < 2) {
printf("usage: hello {start|stop|status}\n");
PX4_WARN("usage: hello {start|stop|status}\n");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (HelloExample::appState.isRunning()) {
printf("already running\n");
PX4_INFO("already running\n");
/* this is not an error */
return 0;
}
@ -86,15 +86,15 @@ int hello_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (HelloExample::appState.isRunning()) {
printf("is running\n");
PX4_INFO("is running\n");
} else {
printf("not started\n");
PX4_INFO("not started\n");
}
return 0;
}
printf("usage: hello_main {start|stop|status}\n");
PX4_WARN("usage: hello_main {start|stop|status}\n");
return 1;
}

42
src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
View File

@ -59,7 +59,7 @@ static int writer_main(int argc, char *argv[])
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
if (fd < 0) {
printf("--- Open failed %d %d", fd, px4_errno);
PX4_INFO("--- Open failed %d %d", fd, px4_errno);
return -px4_errno;
}
@ -67,14 +67,14 @@ static int writer_main(int argc, char *argv[])
int i=0;
while (i<3) {
// Wait for 3 seconds
printf("--- Sleeping for 4 sec\n");
PX4_INFO("--- Sleeping for 4 sec\n");
ret = sleep(4);
if (ret < 0) {
printf("--- sleep failed %d %d\n", ret, errno);
PX4_INFO("--- sleep failed %d %d\n", ret, errno);
return ret;
}
printf("--- writing to fd\n");
PX4_INFO("--- writing to fd\n");
ret = px4_write(fd, buf, 1);
++i;
}
@ -110,16 +110,16 @@ static int test_pub_block(int fd, unsigned long blocked)
{
int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked);
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;
}
ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 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;
}
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;
}
@ -131,29 +131,29 @@ int VCDevExample::main()
_node = new VCDevNode();
if (_node == 0) {
printf("Failed to allocate VCDevNode\n");
PX4_INFO("Failed to allocate VCDevNode\n");
return -ENOMEM;
}
if (_node->init() != PX4_OK) {
printf("Failed to init VCDevNode\n");
PX4_INFO("Failed to init VCDevNode\n");
return 1;
}
int fd = px4_open(TESTDEV, PX4_F_RDONLY);
if (fd < 0) {
printf("Open failed %d %d", fd, px4_errno);
PX4_INFO("Open failed %d %d", fd, px4_errno);
return -px4_errno;
}
void *p = 0;
int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p);
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;
}
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);
if (ret < 0)
@ -174,32 +174,32 @@ int VCDevExample::main()
(char* const*)NULL);
while (!appState.exitRequested() && i<13) {
printf("=====================\n");
printf("==== sleeping 2 sec ====\n");
PX4_INFO("=====================\n");
PX4_INFO("==== sleeping 2 sec ====\n");
sleep(2);
fds[0].fd = fd;
fds[0].events = POLLIN;
fds[0].revents = 0;
printf("==== Calling Poll\n");
PX4_INFO("==== Calling Poll\n");
ret = px4_poll(fds, 1, 1000);
printf("==== Done poll\n");
PX4_INFO("==== Done poll\n");
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);
}
else if (i > 0) {
if (ret == 0)
printf("==== Nothing to read - PASS\n");
PX4_INFO("==== Nothing to read - PASS\n");
else
printf("==== poll returned %d\n", ret);
PX4_INFO("==== poll returned %d\n", ret);
}
else if (i == 0) {
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
printf("==== %d to read - FAIL\n", ret);
PX4_INFO("==== %d to read - FAIL\n", ret);
}
++i;

5
src/platforms/posix/tests/wqueue/wqueue_main.cpp
View File

@ -37,6 +37,7 @@
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_log.h>
#include <px4_middleware.h>
#include <px4_app.h>
#include "wqueue_test.h"
@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "wqueue_test");
printf("wqueue hello\n");
PX4_INFO("wqueue hello\n");
WQueueTest wq;
wq.main();
printf("goodbye\n");
PX4_INFO("goodbye\n");
return 0;
}

11
src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp
View File

@ -39,6 +39,7 @@
*/
#include "wqueue_test.h"
#include <px4_app.h>
#include <px4_log.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
@ -53,14 +54,14 @@ int wqueue_test_main(int argc, char *argv[])
{
if (argc < 2) {
printf("usage: wqueue_test {start|stop|status}\n");
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (WQueueTest::appState.isRunning()) {
printf("already running\n");
PX4_INFO("already running\n");
/* this is not an error */
return 0;
}
@ -82,15 +83,15 @@ int wqueue_test_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (WQueueTest::appState.isRunning()) {
printf("is running\n");
PX4_INFO("is running\n");
} else {
printf("not started\n");
PX4_INFO("not started\n");
}
return 0;
}
printf("usage: wqueue_test {start|stop|status}\n");
PX4_INFO("usage: wqueue_test {start|stop|status}\n");
return 1;
}

2
src/platforms/px4_defines.h
View File

@ -39,7 +39,7 @@
#pragma once
#include <px4_debug.h>
#include <px4_log.h>
/* Get the name of the default value fiven the param name */
#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT

18
src/platforms/px4_debug.h → src/platforms/px4_log.h
View File

@ -32,13 +32,25 @@
****************************************************************************/
/**
* @file px4_debug.h
* Platform dependant debug
* @file px4_log.h
* Platform dependant logging/debug
*/
#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)
#include <err.h>

4
src/platforms/px4_time.h
View File

@ -10,10 +10,13 @@
#elif defined(__PX4_QURT)
#include <sys/timespec.h>
#define CLOCK_REALTIME 1
__BEGIN_DECLS
#if 0
#if !defined(__cplusplus)
struct timespec
{
@ -21,6 +24,7 @@ struct timespec
long tv_nsec;
};
#endif
#endif
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
int px4_clock_settime(clockid_t clk_id, struct timespec *tp);

12
src/platforms/qurt/main.cpp
View File

@ -52,6 +52,9 @@
#include "px4_middleware.h"
static const char *commands =
"x\n"
"hello start"
#if 0
"uorb start\n"
"simulator start -s\n"
"barosim start\n"
@ -68,7 +71,9 @@ static const char *commands =
"hil mode_pwm\n"
"commander start\n"
"list_devices\n"
"list_topics\n";
"list_topics\n"
#endif
;
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)
{
px4::init_once();
px4::init(argc, argv, "mainapp");
process_commands(commands);
for (;;) {}

123
src/platforms/qurt/px4_layer/drv_hrt.c
View File

@ -37,9 +37,12 @@
* High-resolution timer with callouts and timekeeping.
*/
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include <semaphore.h>
#include <time.h>
#include <string.h>
#include <stdio.h>
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);
// Intervals in ms
#define HRT_INTERVAL_MIN 50
#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.
*/
@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void)
{
struct timespec ts;
// FIXME - not supported in QURT
// FIXME - clock_gettime unsupported in QuRT
//clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts);
}
@ -120,7 +142,7 @@ bool hrt_called(struct hrt_call *entry)
*/
void hrt_cancel(struct hrt_call *entry)
{
// FIXME - need a lock
hrt_lock();
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
@ -128,6 +150,7 @@ void hrt_cancel(struct hrt_call *entry)
* being re-entered when the callout returns.
*/
entry->period = 0;
hrt_unlock();
// endif
}
@ -156,7 +179,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
*/
void hrt_init(void)
{
//printf("hrt_init\n");
sq_init(&callout_queue);
sem_init(&_hrt_lock, 0, 1);
memset(&_hrt_work, 0, sizeof(_hrt_work));
}
static void
@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
//printf("hrt_call_enter\n");
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == NULL) || (entry->deadline < call->deadline)) {
@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry)
//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.
*
@ -198,7 +246,10 @@ hrt_call_reschedule()
hrt_abstime now = hrt_absolute_time();
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
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.
*
@ -216,18 +267,26 @@ hrt_call_reschedule()
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
//lldbg("pre-expired\n");
/* 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) {
//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
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 */
/* note that we are using a potentially uninitialised
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;
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)
{
//printf("hrt_call_after\n");
hrt_call_internal(entry,
hrt_absolute_time() + delay,
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);
#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();
}

10
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
View File

@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000;
void usleep(useconds_t usec) {}
unsigned int sleep(unsigned int sec) { return 0; }
extern void hrt_init(void);
__END_DECLS
extern struct wqueue_s gwork[NWORKERS];
@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS];
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)
{
printf("App name: %s\n", app_name);

166
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
View File

@ -33,11 +33,10 @@
****************************************************************************/
/**
* @file px4_qurt_tasks.c
* @file px4_linux_tasks.c
* Implementation of existing task API for Linux
*/
#include <px4_debug.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
@ -54,17 +53,20 @@
#include <string>
#include <px4_tasks.h>
#include <px4_log.h>
#include <hexagon_standalone.h>
#define MAX_CMD_LEN 100
#define PX4_MAX_TASKS 100
#define PX4_MAX_TASKS 5
struct task_entry
{
pthread_t pid;
int pid;
std::string name;
bool isused;
task_entry() : isused(false) {}
void *sp;
};
static task_entry taskmap[PX4_MAX_TASKS];
@ -73,32 +75,31 @@ typedef struct
{
px4_main_t entry;
int argc;
char *argv[];
// strings are allocated after the
char * argv[];
// strings are allocated after
} pthdata_t;
static void *entry_adapter ( void *ptr )
static void entry_adapter ( void *ptr )
{
PX4_DBG("entry_adapter\n");
pthdata_t *data;
data = (pthdata_t *) ptr;
pthdata_t *data = (pthdata_t *) ptr;
PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]);
PX4_DEBUG("data->entry = %p\n", data->entry);
data->entry(data->argc, data->argv);
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_DBG("After px4_task_exit");
return NULL;
PX4_DEBUG("After px4_task_exit\n");
}
void
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 argc = 0;
@ -108,10 +109,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
unsigned long structsize;
char * p = (char *)argv;
pthread_t task;
pthread_attr_t attr;
struct sched_param param;
PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]);
// Calculate argc
while (p != (char *)0) {
p = argv[argc];
@ -120,18 +118,24 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
++argc;
len += strlen(p)+1;
}
PX4_DEBUG("arg %d %p\n", argc, argv);
structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
pthdata_t *taskdata;
PX4_DEBUG("arg %d %p\n", argc, argv);
// not safe to pass stack data to the thread creation
taskdata = (pthdata_t *)malloc(structsize+len);
PX4_DEBUG("arg %d %p\n", argc, argv);
offset = ((unsigned long)taskdata)+structsize;
PX4_DEBUG("arg %d %p\n", argc, argv);
taskdata->entry = entry;
taskdata->argc = argc;
PX4_DEBUG("arg %d %p\n", argc, argv);
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;
strcpy((char *)offset, argv[i]);
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
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, &param);
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) {
if (taskmap[i].isused == false) {
taskmap[i].pid = task;
taskmap[i].pid = i+1;
taskmap[i].name = name;
taskmap[i].isused = true;
//taskmap[i].sp = malloc(stack_size);
taskmap[i].sp = malloc(2048);
break;
}
}
if (i>=PX4_MAX_TASKS) {
return -ENOSPC;
}
return i;
PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]);
thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata);
return i+1;
}
int px4_task_delete(px4_task_t id)
{
int rv = 0;
pthread_t pid;
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;
PX4_DEBUG("Called px4_task_delete\n");
return -EINVAL;
}
void px4_task_exit(int ret)
{
int i;
pthread_t pid = pthread_self();
thread_stop();
// Get pthread ID from the opaque ID
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);
// Free stack
}
int px4_task_kill(px4_task_t id, int sig)
{
int rv = 0;
pthread_t pid;
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;
PX4_DEBUG("Called px4_task_kill\n");
return -EINVAL;
}
void px4_show_tasks()
@ -264,16 +182,17 @@ void px4_show_tasks()
int idx;
int count = 0;
PX4_INFO("Active Tasks:");
PX4_DEBUG("Active Tasks:\n");
for (idx=0; idx < PX4_MAX_TASKS; idx++)
{
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++;
}
}
if (count == 0)
PX4_INFO(" No running tasks");
PX4_DEBUG(" No running tasks\n");
}
// STUBS
@ -282,4 +201,7 @@ extern "C" {
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; }

53
src/platforms/qurt/px4_layer/work_lock.h Normal file
View File

@ -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]);
}

7
src/platforms/qurt/px4_layer/work_queue.c
View File

@ -43,7 +43,10 @@
#include <signal.h>
#include <stdint.h>
#include <queue.h>
#include <stdio.h>
#include <semaphore.h>
#include <px4_workqueue.h>
#include "work_lock.h"
#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.
*/
//flags = irqsave();
work_lock(qid);
work->qtime = clock_systimer(); /* Time work queued */
dq_addlast((dq_entry_t *)work, &wqueue->q);
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
//irqrestore(flags);
work_unlock(qid);
return PX4_OK;
}

62
src/platforms/qurt/px4_layer/work_thread.c
View File

@ -39,12 +39,14 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_time.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <queue.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include "work_lock.h"
#ifdef CONFIG_SCHED_WORKQUEUE
@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS];
/****************************************************************************
* Private Variables
****************************************************************************/
sem_t _work_lock[NWORKERS];
/****************************************************************************
* 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;
worker_t worker;
//irqstate_t flags;
FAR void *arg;
uint32_t elapsed;
uint64_t elapsed;
uint32_t remaining;
uint32_t next;
@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
*/
next = CONFIG_SCHED_WORKPERIOD;
//flags = irqsave();
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
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.
*/
elapsed = clock_systimer() - work->qtime;
//printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
elapsed = USEC2TICK(clock_systimer() - work->qtime);
//printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
if (elapsed >= work->delay)
{
/* 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!
*/
//irqrestore(flags);
work_unlock(lock_id);
if (!worker) {
printf("MESSED UP: worker = 0\n");
PX4_WARN("MESSED UP: worker = 0\n");
}
else
worker(arg);
@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
* back at the head of the list.
*/
//flags = irqsave();
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
}
else
@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
*/
/* Here: elapsed < work->delay */
remaining = work->delay - elapsed;
remaining = USEC_PER_TICK*(work->delay - elapsed);
if (remaining < next)
{
/* 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
* the time elapses or until we are awakened by a signal.
*/
work_unlock(lock_id);
//FIXME - DSPAL doesn't support usleep
//usleep(next);
//irqrestore(flags);
usleep(next);
}
/****************************************************************************
* 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
*
@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[])
* 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 */
@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[])
* 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 */
@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[])
* 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 */
@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[])
uint32_t clock_systimer()
{
//printf("clock_systimer: %0lx\n", hrt_absolute_time());
return (0x00000000ffffffff & hrt_absolute_time());
}
#endif /* CONFIG_SCHED_WORKQUEUE */

4
src/platforms/qurt/tests/hello/hello_example.cpp
View File

@ -1,4 +1,3 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
@ -40,6 +39,7 @@
*/
#include "hello_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
@ -52,7 +52,7 @@ int HelloExample::main()
int i=0;
while (!appState.exitRequested() && i<5) {
printf(" Doing work...\n");
PX4_DEBUG(" Doing work...\n");
++i;
}

5
src/platforms/qurt/tests/hello/hello_main.cpp
View File

@ -38,6 +38,7 @@
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "hello_example.h"
#include <stdio.h>
@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "hello");
printf("hello\n");
PX4_DEBUG("hello\n");
HelloExample hello;
hello.main();
printf("goodbye\n");
PX4_DEBUG("goodbye\n");
return 0;
}

14
src/platforms/qurt/tests/hello/hello_start_qurt.cpp
View File

@ -38,6 +38,7 @@
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "hello_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
@ -52,11 +53,11 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]);
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[])
{
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) {
usage();
return 1;
@ -64,8 +65,9 @@ int hello_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
PX4_DEBUG("Starting\n");
if (HelloExample::appState.isRunning()) {
printf("already running\n");
PX4_DEBUG("already running\n");
/* this is not an error */
return 0;
}
@ -73,7 +75,7 @@ int hello_main(int argc, char *argv[])
daemon_task = px4_task_spawn_cmd("hello",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
16000,
PX4_MAIN,
(char* const*)argv);
@ -87,10 +89,10 @@ int hello_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (HelloExample::appState.isRunning()) {
printf("is running\n");
PX4_DEBUG("is running\n");
} else {
printf("not started\n");
PX4_DEBUG("not started\n");
}
return 0;