/**
* @brief Initializes the lwIP subsystem.
* @note The function exits after the initialization is finished.
*
* @param[in] opts pointer to the configuration structure, if @p NULL
* then the static configuration is used.
*/
void lwipInit(const lwipthread_opts_t *opts) {
/* Creating the lwIP thread (it changes priority internally).*/
chThdCreateStatic(wa_lwip_thread, LWIP_THREAD_STACK_SIZE,
chThdGetPriorityX() - 1, lwip_thread, (void *)opts);
/* Waiting for the lwIP thread complete initialization. Note,
this thread reaches the thread reference object first because
the relative priorities.*/
chSysLock();
chThdSuspendS(&lwip_trp);
chSysUnlock();
}
static void test_002_003_execute(void) {
tprio_t prio, p1;
/* [2.3.1] Thread priority is increased by one then a check is
performed.*/
test_set_step(1);
{
prio = chThdGetPriorityX();
p1 = chThdSetPriority(prio + 1);
test_assert(p1 == prio, "unexpected returned priority level");
test_assert(chThdGetPriorityX() == prio + 1, "unexpected priority level");
}
/* [2.3.2] Thread priority is returned to the previous value then a
check is performed.*/
test_set_step(2);
{
p1 = chThdSetPriority(p1);
test_assert(p1 == prio + 1, "unexpected returned priority level");
test_assert(chThdGetPriorityX() == prio, "unexpected priority level");
}
}
static void test_005_009_execute(void) {
tprio_t prio;
/* [5.9.1] Reading current base priority.*/
test_set_step(1);
{
prio = chThdGetPriorityX();
}
/* [5.9.2] Thread A is created at priority P(+1), it locks M2, locks
M1 and goes to wait on C1.*/
test_set_step(2);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread8, "A");
}
/* [5.9.3] Thread C is created at priority P(+2), it enqueues on M1
and boosts TA priority at P(+2).*/
test_set_step(3);
{
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread6, "C");
}
/* [5.9.4] Thread B is created at priority P(+3), it enqueues on M2
and boosts TA priority at P(+3).*/
test_set_step(4);
{
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread9, "B");
}
/* [5.9.5] Signaling C1: TA wakes up, unlocks M1 and priority goes to
P(+2). TB locks M1, unlocks M1 and completes. TA unlocks M2 and
priority goes to P(+1). TC waits on C1. TA completes.*/
test_set_step(5);
{
chCondSignal(&c1);
}
/* [5.9.6] Signaling C1: TC wakes up, unlocks M1 and completes.*/
test_set_step(6);
{
chCondSignal(&c1);
}
/* [5.9.7] Checking the order of operations.*/
test_set_step(7);
{
test_wait_threads();
test_assert_sequence("ABC", "invalid sequence");
}
}
bool fiveBaudInit(SerialDriver *sd)
{
uint8_t input_buf[3];
size_t bytes;
// Set pin mode to GPIO
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_INPUT_ANALOG);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
const uint8_t prio = chThdGetPriorityX();
chThdSetPriority(HIGHPRIO);
K_HIGH(320); // As per ISO standard
// Send 0x33 at 5 baud (00110011)
// K-line level: |_--__--__-|
K_LOW(200); // Low for 200ms - start bit
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(200); // High for 200ms - stop bit
// Set pin mode back to UART
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUPDR_PULLUP);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OTYPE_OPENDRAIN);
chThdSetPriority(prio); // Revert back original priority
chThdSleepMilliseconds(25);
bytes = sdReadTimeout(sd, input_buf, sizeof(input_buf), MS2ST(500)); // 300ms max according to ISO9141
if (bytes != 3 || input_buf[0] != 0x55)
{
return false;
}
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
uint8_t key = input_buf[2] ^ 0xFF; // Invert key byte
sdWriteTimeout(sd, &key, 1, MS2ST(100));
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
bytes = sdReadTimeout(sd, input_buf, 1, MS2ST(100));
if (bytes != 1 || input_buf[0] != 0xCC)
{
return false;
}
return true;
}
static void test_005_001_execute(void) {
tprio_t prio;
/* [5.1.1] Getting the initial priority.*/
test_set_step(1);
{
prio = chThdGetPriorityX();
}
/* [5.1.2] Locking the mutex.*/
test_set_step(2);
{
chMtxLock(&m1);
}
/* [5.1.3] Five threads are created that try to lock and unlock the
mutex then terminate. The threads are created in ascending
priority order.*/
test_set_step(3);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread1, "E");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread1, "D");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread1, "C");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prio+4, thread1, "B");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prio+5, thread1, "A");
}
/* [5.1.4] Unlocking the mutex, the threads will wakeup in priority
order because the mutext queue is an ordered one.*/
test_set_step(4);
{
chMtxUnlock(&m1);
test_wait_threads();
test_assert(prio == chThdGetPriorityX(), "wrong priority level");
test_assert_sequence("ABCDE", "invalid sequence");
}
}
static void queues2_execute(void) {
unsigned i;
size_t n;
/* Initial empty state */
test_assert_lock(1, chOQIsEmptyI(&oq), "not empty");
/* Queue filling */
for (i = 0; i < TEST_QUEUES_SIZE; i++)
chOQPut(&oq, 'A' + i);
test_assert_lock(2, chOQIsFullI(&oq), "still has space");
/* Queue emptying */
for (i = 0; i < TEST_QUEUES_SIZE; i++) {
char c;
chSysLock();
c = chOQGetI(&oq);
chSysUnlock();
test_emit_token(c);
}
test_assert_lock(3, chOQIsEmptyI(&oq), "still full");
test_assert_sequence(4, "ABCD");
test_assert_lock(5, chOQGetI(&oq) == Q_EMPTY, "failed to report Q_EMPTY");
/* Writing the whole thing */
n = chOQWriteTimeout(&oq, wa[1], TEST_QUEUES_SIZE * 2, TIME_IMMEDIATE);
test_assert(6, n == TEST_QUEUES_SIZE, "wrong returned size");
test_assert_lock(7, chOQIsFullI(&oq), "not full");
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread2, NULL);
test_assert_lock(8, chOQGetFullI(&oq) == TEST_QUEUES_SIZE, "not empty");
test_wait_threads();
/* Testing reset */
chSysLock();
chOQResetI(&oq);
chSysUnlock();
test_assert_lock(9, chOQGetFullI(&oq) == 0, "still full");
/* Partial writes */
n = chOQWriteTimeout(&oq, wa[1], TEST_QUEUES_SIZE / 2, TIME_IMMEDIATE);
test_assert(10, n == TEST_QUEUES_SIZE / 2, "wrong returned size");
n = chOQWriteTimeout(&oq, wa[1], TEST_QUEUES_SIZE / 2, TIME_IMMEDIATE);
test_assert(11, n == TEST_QUEUES_SIZE / 2, "wrong returned size");
test_assert_lock(12, chOQIsFullI(&oq), "not full");
/* Timeout */
test_assert(13, chOQPutTimeout(&oq, 0, 10) == Q_TIMEOUT, "wrong timeout return");
}
static void cmd_test(BaseSequentialStream *chp, int argc, char *argv[]) {
thread_t *tp;
(void)argv;
if (argc > 0) {
chprintf(chp, "Usage: test\r\n");
return;
}
tp = chThdCreateFromHeap(NULL, TEST_WA_SIZE, chThdGetPriorityX(),
TestThread, chp);
if (tp == NULL) {
chprintf(chp, "out of memory\r\n");
return;
}
chThdWait(tp);
}
static void rt_test_005_003_execute(void) {
unsigned i;
systime_t target_time;
msg_t msg;
/* [5.3.1] Testing special case TIME_IMMEDIATE.*/
test_set_step(1);
{
msg = chSemWaitTimeout(&sem1, TIME_IMMEDIATE);
test_assert(msg == MSG_TIMEOUT, "wrong wake-up message");
test_assert(queue_isempty(&sem1.queue), "queue not empty");
test_assert(sem1.cnt == 0, "counter not zero");
}
/* [5.3.2] Testing non-timeout condition.*/
test_set_step(2);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, 0);
msg = chSemWaitTimeout(&sem1, TIME_MS2I(500));
test_wait_threads();
test_assert(msg == MSG_OK, "wrong wake-up message");
test_assert(queue_isempty(&sem1.queue), "queue not empty");
test_assert(sem1.cnt == 0, "counter not zero");
}
/* [5.3.3] Testing timeout condition.*/
test_set_step(3);
{
target_time = chTimeAddX(test_wait_tick(), TIME_MS2I(5 * 50));
for (i = 0; i < 5; i++) {
test_emit_token('A' + i);
msg = chSemWaitTimeout(&sem1, TIME_MS2I(50));
test_assert(msg == MSG_TIMEOUT, "wrong wake-up message");
test_assert(queue_isempty(&sem1.queue), "queue not empty");
test_assert(sem1.cnt == 0, "counter not zero");
}
test_assert_sequence("ABCDE", "invalid sequence");
test_assert_time_window(target_time,
chTimeAddX(target_time, ALLOWED_DELAY),
"out of time window");
}
}
static void mtx6_execute(void) {
tprio_t prio = chThdGetPriorityX();
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread10, "E");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread10, "D");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread10, "C");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prio+4, thread10, "B");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prio+5, thread10, "A");
chSysLock();
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chSchRescheduleS();
chSysUnlock();
test_wait_threads();
test_assert_sequence(1, "ABCDE");
}
static void rt_test_005_004_execute(void) {
/* [5.4.1] A thread is created, it goes to wait on the semaphore.*/
test_set_step(1);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread1, "A");
}
/* [5.4.2] The semaphore counter is increased by two, it is then
tested to be one, the thread must have completed.*/
test_set_step(2);
{
chSysLock();
chSemAddCounterI(&sem1, 2);
chSchRescheduleS();
chSysUnlock();
test_wait_threads();
test_assert_lock(chSemGetCounterI(&sem1) == 1, "invalid counter");
test_assert_sequence("A", "invalid sequence");
}
}
static void msg1_execute(void) {
thread_t *tp;
msg_t msg;
/*
* Testing the whole messages loop.
*/
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() + 1,
thread, chThdGetSelfX());
tp = chMsgWait();
msg = chMsgGet(tp);
chMsgRelease(tp, msg);
test_emit_token(msg);
tp = chMsgWait();
msg = chMsgGet(tp);
chMsgRelease(tp, msg);
test_emit_token(msg);
tp = chMsgWait();
msg = chMsgGet(tp);
chMsgRelease(tp, msg);
test_emit_token(msg);
test_assert_sequence(1, "ABC");
}
static void test_005_008_execute(void) {
/* [5.8.1] Starting the five threads with increasing priority, the
threads will queue on the condition variable.*/
test_set_step(1);
{
tprio_t prio = chThdGetPriorityX();
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread6, "E");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread6, "D");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread6, "C");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prio+4, thread6, "B");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prio+5, thread6, "A");
}
/* [5.8.2] Broarcasting on the condition variable then waiting for
the threads to terminate in priority order, the order is tested.*/
test_set_step(2);
{
chCondBroadcast(&c1);
test_wait_threads();
test_assert_sequence("ABCDE", "invalid sequence");
}
}
static void cmd_benchmark( int argc, char *argv[] )
{
thread_t *tp;
(void)argv;
if( argc > 0 )
{
usage( "benchmark" );
return;
}
tp = chThdCreateFromHeap( NULL,
TEST_WA_SIZE,
chThdGetPriorityX(),
TestThread,
&SDU2 );
if( tp == NULL )
{
chprint( "out of memory\r\n" );
return;
}
chThdWait( tp );
}
static void dyn1_execute(void) {
size_t n, sz;
void *p1;
tprio_t prio = chThdGetPriorityX();
(void)chHeapStatus(&heap1, &sz);
/* Starting threads from the heap. */
threads[0] = chThdCreateFromHeap(&heap1,
THD_WORKING_AREA_SIZE(THREADS_STACK_SIZE),
prio-1, thread, "A");
threads[1] = chThdCreateFromHeap(&heap1,
THD_WORKING_AREA_SIZE(THREADS_STACK_SIZE),
prio-2, thread, "B");
/* Allocating the whole heap in order to make the thread creation fail.*/
(void)chHeapStatus(&heap1, &n);
p1 = chHeapAlloc(&heap1, n);
threads[2] = chThdCreateFromHeap(&heap1,
THD_WORKING_AREA_SIZE(THREADS_STACK_SIZE),
prio-3, thread, "C");
chHeapFree(p1);
test_assert(1, (threads[0] != NULL) &&
(threads[1] != NULL) &&
(threads[2] == NULL) &&
(threads[3] == NULL) &&
(threads[4] == NULL),
"thread creation failed");
/* Claiming the memory from terminated threads. */
test_wait_threads();
test_assert_sequence(2, "AB");
/* Heap status checked again.*/
test_assert(3, chHeapStatus(&heap1, &n) == 1, "heap fragmented");
test_assert(4, n == sz, "heap size changed");
}
static void test_002_002_execute(void) {
/* [2.2.1] Creating 5 threads with increasing priority, execution
sequence is tested.*/
test_set_step(1);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");
test_wait_threads();
test_assert_sequence("ABCDE", "invalid sequence");
}
/* [2.2.2] Creating 5 threads with decreasing priority, execution
sequence is tested.*/
test_set_step(2);
{
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");
test_wait_threads();
test_assert_sequence("ABCDE", "invalid sequence");
}
/* [2.2.3] Creating 5 threads with pseudo-random priority, execution
sequence is tested.*/
test_set_step(3);
{
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-4, thread, "D");
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-5, thread, "E");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, chThdGetPriorityX()-1, thread, "A");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, chThdGetPriorityX()-2, thread, "B");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread, "C");
test_wait_threads();
test_assert_sequence("ABCDE", "invalid sequence");
}
}
static void test_005_004_execute(void) {
tprio_t p, pa, pb;
/* [5.4.1] Getting current thread priority P(0) and assigning to the
threads A and B priorities +1 and +2.*/
test_set_step(1);
{
p = chThdGetPriorityX();
pa = p + 1;
pb = p + 2;
}
/* [5.4.2] Spawning threads A and B at priorities P(A) and P(B).*/
test_set_step(2);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, pa, thread4A, "A");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, pb, thread4B, "B");
}
/* [5.4.3] Locking the mutex M1 before thread A has a chance to lock
it. The priority must not change because A has not yet reached
chMtxLock(M1). the mutex is not locked.*/
test_set_step(3);
{
chMtxLock(&m1);
test_assert(chThdGetPriorityX() == p, "wrong priority level");
}
/* [5.4.4] Waiting 100mS, this makes thread A reach chMtxLock(M1) and
get the mutex. This must boost the priority of the current thread
at the same level of thread A.*/
test_set_step(4);
{
chThdSleepMilliseconds(100);
test_assert(chThdGetPriorityX() == pa, "wrong priority level");
}
/* [5.4.5] Locking the mutex M2 before thread B has a chance to lock
it. The priority must not change because B has not yet reached
chMtxLock(M2). the mutex is not locked.*/
test_set_step(5);
{
chMtxLock(&m2);
test_assert(chThdGetPriorityX() == pa, "wrong priority level");
}
/* [5.4.6] Waiting 100mS, this makes thread B reach chMtxLock(M2) and
get the mutex. This must boost the priority of the current thread
at the same level of thread B.*/
test_set_step(6);
{
chThdSleepMilliseconds(100);
test_assert(chThdGetPriorityX() == pb, "wrong priority level");
}
/* [5.4.7] Unlocking M2, the priority should fall back to P(A).*/
test_set_step(7);
{
chMtxUnlock(&m2);
test_assert(chThdGetPriorityX() == pa, "wrong priority level");
}
/* [5.4.8] Unlocking M1, the priority should fall back to P(0).*/
test_set_step(8);
{
chMtxUnlock(&m1);
test_assert(chThdGetPriorityX() == p, "wrong priority level");
}
}
static void test_005_006_execute(void) {
bool b;
tprio_t prio;
/* [5.6.1] Getting current thread priority for later checks.*/
test_set_step(1);
{
prio = chThdGetPriorityX();
}
/* [5.6.2] Locking the mutex first time, it must be possible because
it is not owned.*/
test_set_step(2);
{
b = chMtxTryLock(&m1);
test_assert(b, "already locked");
}
/* [5.6.3] Locking the mutex second time, it must be possible because
it is recursive.*/
test_set_step(3);
{
b = chMtxTryLock(&m1);
test_assert(b, "already locked");
}
/* [5.6.4] Unlocking the mutex then it must be still owned because
recursivity.*/
test_set_step(4);
{
chMtxUnlock(&m1);
test_assert(m1.owner != NULL, "not owned");
}
/* [5.6.5] Unlocking the mutex then it must not be owned anymore and
the queue must be empty.*/
test_set_step(5);
{
chMtxUnlock(&m1);
test_assert(m1.owner == NULL, "still owned");
test_assert(queue_isempty(&m1.queue), "queue not empty");
}
/* [5.6.6] Testing that priority has not changed after operations.*/
test_set_step(6);
{
test_assert(chThdGetPriorityX() == prio, "wrong priority level");
}
/* [5.6.7] Testing consecutive chMtxTryLock()/chMtxTryLockS() calls
and a final chMtxUnlockAllS().*/
test_set_step(7);
{
b = chMtxTryLock(&m1);
test_assert(b, "already locked");
chSysLock();
b = chMtxTryLockS(&m1);
chSysUnlock();
test_assert(b, "already locked");
test_assert(m1.cnt == 2, "invalid recursion counter");
chSysLock();
chMtxUnlockAllS();
chSysUnlock();
test_assert(m1.owner == NULL, "still owned");
test_assert(queue_isempty(&m1.queue), "queue not empty");
test_assert(m1.cnt == 0, "invalid recursion counter");
}
/* [5.6.8] Testing consecutive chMtxLock()/chMtxLockS() calls and a
final chMtxUnlockAll().*/
test_set_step(8);
{
chMtxLock(&m1);
test_assert(m1.owner != NULL, "not owned");
chSysLock();
chMtxLockS(&m1);
chSysUnlock();
test_assert(m1.owner != NULL, "not owned");
test_assert(m1.cnt == 2, "invalid recursion counter");
chMtxUnlockAll();
test_assert(m1.owner == NULL, "still owned");
test_assert(queue_isempty(&m1.queue), "queue not empty");
test_assert(m1.cnt == 0, "invalid recursion counter");
}
/* [5.6.9] Testing that priority has not changed after operations.*/
test_set_step(9);
{
test_assert(chThdGetPriorityX() == prio, "wrong priority level");
}
}
static void mtx4_execute(void) {
tprio_t p, p1, p2;
p = chThdGetPriorityX();
p1 = p + 1;
p2 = p + 2;
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, p1, thread4a, "B");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, p2, thread4b, "A");
chMtxLock(&m2);
test_assert(1, chThdGetPriorityX() == p, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(2, chThdGetPriorityX() == p1, "wrong priority level");
chMtxLock(&m1);
test_assert(3, chThdGetPriorityX() == p1, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(4, chThdGetPriorityX() == p2, "wrong priority level");
chMtxUnlock(&m1);
test_assert(5, chThdGetPriorityX() == p1, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(6, chThdGetPriorityX() == p1, "wrong priority level");
chMtxUnlockAll();
test_assert(7, chThdGetPriorityX() == p, "wrong priority level");
test_wait_threads();
/* Test repeated in order to cover chMtxUnlockS().*/
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, p1, thread4a, "D");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, p2, thread4b, "C");
chMtxLock(&m2);
test_assert(8, chThdGetPriorityX() == p, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(9, chThdGetPriorityX() == p1, "wrong priority level");
chMtxLock(&m1);
test_assert(10, chThdGetPriorityX() == p1, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(11, chThdGetPriorityX() == p2, "wrong priority level");
chSysLock();
chMtxUnlockS(&m1);
chSchRescheduleS();
chSysUnlock();
test_assert(12, chThdGetPriorityX() == p1, "wrong priority level");
chThdSleepMilliseconds(100);
test_assert(13, chThdGetPriorityX() == p1, "wrong priority level");
chMtxUnlockAll();
test_assert(14, chThdGetPriorityX() == p, "wrong priority level");
test_wait_threads();
}
static bool do_bemf_spinup(const float max_duty_cycle, const unsigned num_prior_attempts)
{
assert(chThdGetPriorityX() == HIGHPRIO); // Mandatory
static const unsigned POWER_MULT_MAX = 5;
unsigned power_multiplier = num_prior_attempts + 1;
if (power_multiplier > POWER_MULT_MAX) {
power_multiplier = POWER_MULT_MAX;
}
assert(power_multiplier >= 1);
assert(power_multiplier <= POWER_MULT_MAX);
// Make sure we're not going to underflow during time calculations
while (motor_timer_hnsec() < _params.spinup_start_comm_period) {
;
}
const uint64_t deadline = motor_timer_hnsec() + _params.spinup_timeout;
float dc = _params.spinup_duty_cycle_increment;
unsigned num_good_comms = 0;
_state.comm_period = _params.spinup_start_comm_period;
_state.prev_zc_timestamp = motor_timer_hnsec() - _state.comm_period / 2;
_state.pwm_val = motor_pwm_compute_pwm_val(dc);
while (motor_timer_hnsec() <= deadline) {
// Engage the current comm step
irq_primask_disable();
motor_pwm_set_step_from_isr(_state.comm_table + _state.current_comm_step, _state.pwm_val);
irq_primask_enable();
uint64_t step_deadline = motor_timer_hnsec() + _state.comm_period;
motor_timer_hndelay(_params.comm_blank_hnsec);
// Wait for the next zero crossing
const uint64_t zc_timestamp = spinup_wait_zc(step_deadline);
num_good_comms = (zc_timestamp > 0) ? (num_good_comms + 1) : 0;
// Compute the next duty cycle
dc += _params.spinup_duty_cycle_increment;
if (dc > max_duty_cycle * power_multiplier) {
dc = max_duty_cycle * power_multiplier;
}
_state.pwm_val = motor_pwm_compute_pwm_val(dc);
if (zc_timestamp > 0) {
assert(zc_timestamp > _state.prev_zc_timestamp);
// Update comm period
const uint32_t new_comm_period = zc_timestamp - _state.prev_zc_timestamp;
_state.prev_zc_timestamp = zc_timestamp;
_state.comm_period = (_state.comm_period + new_comm_period) / 2;
step_deadline = zc_timestamp + _state.comm_period / 2;
// Check the termination condition
const bool enough_good_comms = num_good_comms > _params.spinup_num_good_comms;
const bool fast_enough = _state.comm_period <= _params.spinup_end_comm_period;
if (enough_good_comms || fast_enough) {
break;
}
} else {
// If ZC was not detected, we need to fake the previous ZC timestamp now
_state.prev_zc_timestamp = step_deadline - _state.comm_period / 2;
}
// Wait till the end of the current step
while (motor_timer_hnsec() <= step_deadline) {
;
}
// Next step
_state.current_comm_step++;
if (_state.current_comm_step >= MOTOR_NUM_COMMUTATION_STEPS) {
_state.current_comm_step = 0;
}
}
return _state.comm_period < _params.comm_period_max;
}
static void evt2_execute(void) {
eventmask_t m;
event_listener_t el1, el2;
systime_t target_time;
/*
* Test on chEvtWaitOne() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitOne(ALL_EVENTS);
test_assert(1, m == 1, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(2, m == 4, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(3, m == 0, "stuck event");
/*
* Test on chEvtWaitOne() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitOne(ALL_EVENTS);
test_assert_time_window(4, target_time, target_time + ALLOWED_DELAY);
test_assert(5, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(6, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAny() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitAny(ALL_EVENTS);
test_assert(7, m == 5, "unexpected pending bit");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(8, m == 0, "stuck event");
/*
* Test on chEvtWaitAny() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitAny(ALL_EVENTS);
test_assert_time_window(9, target_time, target_time + ALLOWED_DELAY);
test_assert(10, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(11, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAll().
*/
chEvtObjectInit(&es1);
chEvtObjectInit(&es2);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es2, &el2, 4);
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, "A");
m = chEvtWaitAll(5);
test_assert_time_window(12, target_time, target_time + ALLOWED_DELAY);
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(13, m == 0, "stuck event");
test_wait_threads();
chEvtUnregister(&es1, &el1);
chEvtUnregister(&es2, &el2);
test_assert(14, !chEvtIsListeningI(&es1), "stuck listener");
test_assert(15, !chEvtIsListeningI(&es2), "stuck listener");
}
static void control_thread(void* arg)
{
(void)arg;
chRegSetThreadName("motor");
event_listener_t listener;
chEvtRegisterMask(&_setpoint_update_event, &listener, ALL_EVENTS);
uint64_t timestamp_hnsec = motor_rtctl_timestamp_hnsec();
while (1) {
/*
* Control loop period adapts to comm period.
*/
const uint32_t comm_period = motor_rtctl_get_comm_period_hnsec();
unsigned control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
if (comm_period > 0) {
control_period_ms = comm_period / HNSEC_PER_MSEC;
}
if (control_period_ms < 1) {
control_period_ms = 1;
} else if (control_period_ms > IDLE_CONTROL_PERIOD_MSEC) {
control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
}
/*
* Thread priority - maximum if the motor is running, normal otherwise
*/
const tprio_t desired_thread_priority = (comm_period > 0) ? HIGHPRIO : NORMALPRIO;
if (desired_thread_priority != chThdGetPriorityX()) {
const tprio_t old = chThdSetPriority(desired_thread_priority);
printf("Motor: Priority changed: %i --> %i\n", (int)old, (int)desired_thread_priority);
}
/*
* The event must be set only when the mutex is unlocked.
* Otherwise this thread will take control, stumble upon the locked mutex, return the control
* to the thread that holds the mutex, unlock the mutex, then proceed.
*/
chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(control_period_ms));
chMtxLock(&_mutex);
const uint64_t new_timestamp_hnsec = motor_rtctl_timestamp_hnsec();
const uint32_t dt_hnsec = new_timestamp_hnsec - timestamp_hnsec;
const float dt = dt_hnsec / (float)HNSEC_PER_SEC;
timestamp_hnsec = new_timestamp_hnsec;
assert(dt > 0);
update_filters(dt);
update_setpoint_ttl(dt_hnsec / HNSEC_PER_MSEC);
update_control(comm_period, dt);
poll_beep();
chMtxUnlock(&_mutex);
watchdogReset(_watchdog_id);
}
abort();
}
