mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +08:00
Qurt platform custom icm42688p IMU driver (#20753)
- first version of IMU driver for the VOXL 2 platform (Qurt) - this is a customized version of the Invensense ICM42688P driver, it is currently in the VOXL 2 board directory
This commit is contained in:
@@ -126,15 +126,10 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
|
||||
dq_addlast(&work->dq, &wqueue->q);
|
||||
|
||||
if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */
|
||||
#ifdef __PX4_QURT
|
||||
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
|
||||
#else
|
||||
//wqueue->pid == own task? -> don't signal
|
||||
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
|
||||
#endif
|
||||
}
|
||||
|
||||
hrt_work_unlock();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
|
||||
@@ -138,6 +138,16 @@ static void hrt_work_process()
|
||||
/* Default to sleeping for 1 sec */
|
||||
next = 1000000;
|
||||
|
||||
#ifdef __PX4_QURT
|
||||
// In Posix certain signals wake up a sleeping thread but it isn't the case
|
||||
// with the Qurt POSIX implementation. So rather than assume we can come out
|
||||
// of the sleep early by a signal we just wake up more often. The best way to
|
||||
// fix this would be to move to a px4_sem_timedwait. But the current implementation
|
||||
// of that function on Qurt uses this hrt_thread! So, it would all have to be
|
||||
// re-written to use Qurt semaphores which do have timed waits.
|
||||
next = 1000;
|
||||
#endif
|
||||
|
||||
hrt_work_lock();
|
||||
|
||||
work = (struct work_s *)wqueue->q.head;
|
||||
@@ -287,9 +297,5 @@ void hrt_work_queue_init(void)
|
||||
(char *const *)NULL);
|
||||
|
||||
|
||||
#ifdef __PX4_QURT
|
||||
signal(SIGALRM, _sighandler);
|
||||
#else
|
||||
signal(SIGCONT, _sighandler);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -41,6 +41,7 @@
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/workqueue.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
|
||||
#include <signal.h>
|
||||
#include <stdint.h>
|
||||
@@ -125,11 +126,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
|
||||
work->qtime = clock_systimer(); /* Time work queued */
|
||||
|
||||
dq_addlast((dq_entry_t *)work, &wqueue->q);
|
||||
#ifdef __PX4_QURT
|
||||
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
|
||||
#else
|
||||
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
|
||||
#endif
|
||||
|
||||
work_unlock(qid);
|
||||
return PX4_OK;
|
||||
|
||||
Reference in New Issue
Block a user