/** * @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(); }