/**
* @brief Returns the thread next to the specified one.
* @details The reference counter of the specified thread is decremented and
* the reference counter of the returned thread is incremented.
*
* @param[in] tp pointer to the thread
* @return A reference to the next thread.
* @retval NULL if there is no next thread.
*
* @api
*/
Thread *chRegNextThread(Thread *tp) {
Thread *ntp;
chSysLock();
ntp = tp->p_newer;
if (ntp == (Thread *)&rlist)
ntp = NULL;
#if CH_USE_DYNAMIC
else {
chDbgAssert(ntp->p_refs < 255, "chRegNextThread(), #1",
"too many references");
ntp->p_refs++;
}
#endif
chSysUnlock();
#if CH_USE_DYNAMIC
chThdRelease(tp);
#endif
return ntp;
}
/**
* @brief Input queue read with timeout.
* @details This function reads a byte value from an input queue. If the queue
* is empty then the calling thread is suspended until a byte arrives
* in the queue or a timeout occurs.
*
* @param[in] iqp pointer to an @p InputQueue structure
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return A byte value from the queue.
* @retval Q_TIMEOUT if the specified time expired.
* @retval Q_RESET if the queue was reset.
*
* @api
*/
msg_t chIQGetTimeout(InputQueue *iqp, systime_t time) {
uint8_t b;
msg_t msg;
chSysLock();
if (iqp->q_notify)
iqp->q_notify(iqp);
if ((msg = chSemWaitTimeoutS(&iqp->q_sem, time)) < RDY_OK) {
chSysUnlock();
return msg;
}
b = *iqp->q_rdptr++;
if (iqp->q_rdptr >= iqp->q_top)
iqp->q_rdptr = iqp->q_buffer;
chSysUnlock();
return b;
}
/**
* @brief Waits for all the specified events.
* @details The function waits for all the events specified in @p events to
* become pending then the events are cleared and returned.
*
* @param[in] events events that the function should wait
* for, @p ALL_EVENTS requires all the events
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return The mask of the served and cleared events.
* @retval 0 if the operation has timed out.
*
* @api
*/
eventmask_t chEvtWaitAllTimeout(eventmask_t events, systime_t time) {
thread_t *ctp = currp;
chSysLock();
if ((ctp->epending & events) != events) {
if (TIME_IMMEDIATE == time) {
chSysUnlock();
return (eventmask_t)0;
}
ctp->u.ewmask = events;
if (chSchGoSleepTimeoutS(CH_STATE_WTANDEVT, time) < MSG_OK) {
chSysUnlock();
return (eventmask_t)0;
}
}
ctp->epending &= ~events;
chSysUnlock();
return events;
}
/**
* @brief Terminates the current thread.
* @details The thread goes in the @p THD_STATE_FINAL state holding the
* specified exit status code, other threads can retrieve the
* exit status code by invoking the function @p chThdWait().
* @post Eventual code after this function will never be executed,
* this function never returns. The compiler has no way to
* know this so do not assume that the compiler would remove
* the dead code.
*
* @param[in] msg thread exit code
*
* @api
*/
void chThdExit(msg_t msg) {
Thread *tp = currp;
chSysLock();
tp->p_u.exitcode = msg;
#if defined(THREAD_EXT_EXIT_HOOK)
THREAD_EXT_EXIT_HOOK(tp);
#endif
#if CH_USE_WAITEXIT
while (notempty(&tp->p_waiting))
chSchReadyI(list_remove(&tp->p_waiting));
#endif
#if CH_USE_REGISTRY
/* Static threads are immediately removed from the registry because
there is no memory to recover.*/
if ((tp->p_flags & THD_MEM_MODE_MASK) == THD_MEM_MODE_STATIC)
REG_REMOVE(tp);
#endif
chSchGoSleepS(THD_STATE_FINAL);
}
void RemotePublisher::subscribe(LocalSubscriber * sub, void * buffer,
size_t size) {
chPoolLoadArray(&_pool, buffer, size);
chSysLock();
if (this->_subscribers == NULL) {
this->_subscribers = sub;
} else {
sub->link(_subscribers);
_subscribers = sub;
}
_rtcan_msg.data = (uint8_t *) allocI();
if (_rtcan_msg.data) {
rtcanRegister(&RTCAND, &_rtcan_msg);
}
chSysUnlock();
}
/**
* @brief Unregisters an Event Listener from its Event Source.
* @note If the event listener is not registered on the specified event
* source then the function does nothing.
* @note For optimal performance it is better to perform the unregister
* operations in inverse order of the register operations (elements
* are found on top of the list).
*
* @param[in] esp pointer to the @p event_source_t structure
* @param[in] elp pointer to the @p event_listener_t structure
*
* @api
*/
void chEvtUnregister(event_source_t *esp, event_listener_t *elp) {
event_listener_t *p;
chDbgCheck((esp != NULL) && (elp != NULL));
/*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
p = (event_listener_t *)esp;
/*lint -restore*/
chSysLock();
/*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
while (p->next != (event_listener_t *)esp) {
/*lint -restore*/
if (p->next == elp) {
p->next = elp->next;
break;
}
p = p->next;
}
chSysUnlock();
}
u32 usart_support_write(void *sd, const u8 data[], u32 len)
{
struct usart_tx_dma_state *s = &((struct usart_support_s *)sd)->tx;
/* If there is no data to write, just return. */
if (len == 0) return 0;
chSysLock();
/* Check if the write would cause a buffer overflow, if so only write up to
* the end of the buffer. */
u32 n_free = usart_tx_n_free(sd);
if (len > n_free) {
chSysUnlock();
return 0;
}
u32 old_wr = s->wr;
s->wr = (s->wr + len) % USART_TX_BUFFER_LEN;
if (old_wr + len <= USART_TX_BUFFER_LEN)
memcpy(&(s->buff[old_wr]), data, len);
else {
/* Deal with case where write wraps the buffer. */
memcpy(&(s->buff[old_wr]), &data[0], USART_TX_BUFFER_LEN - old_wr);
memcpy(&(s->buff[0]), &data[USART_TX_BUFFER_LEN - old_wr],
len - (USART_TX_BUFFER_LEN - old_wr));
}
/* Check if there is a DMA transfer either in progress or waiting for its
* interrupt to be serviced. Its very important to also check the interrupt
* flag as EN will be cleared when the transfer finishes but we really need
* to make sure the ISR has been run to finish up the bookkeeping for the
* transfer. Also, make sure that this is done atomically without a DMA
* interrupt squeezing in there. */
if (!s->busy)
dma_schedule(s);
chSysUnlock();
return len;
}
msg_t pwm_node(void * arg) {
uint8_t index = *(reinterpret_cast<uint8_t *>(arg));
r2p::Node node("pwm2sub");
r2p::Subscriber<r2p::PWM2Msg, 5> pwm_sub;
r2p::PWM2Msg * msgp;
(void) arg;
chRegSetThreadName("pwm_node");
/* Enable the h-bridge. */
palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
node.subscribe(pwm_sub, "pwm");
for (;;) {
if (node.spin(r2p::Time::ms(1000))) {
if (pwm_sub.fetch(msgp)) {
pwm = msgp->value[index];
chSysLock()
;
if (pwm >= 0) {
pwm_lld_enable_channel(&PWMD1, 0, msgp->value[index]);
pwm_lld_enable_channel(&PWMD1, 1, 0);
} else {
pwm_lld_enable_channel(&PWMD1, 0, 0);
pwm_lld_enable_channel(&PWMD1, 1, -msgp->value[index]);
}
chSysUnlock();
pwm_sub.release(*msgp);
}
} else {
// Stop motor if no messages for 1000 ms
pwm_lld_disable_channel(&PWM_DRIVER, 0);
pwm_lld_disable_channel(&PWM_DRIVER, 1);
}
}
return CH_SUCCESS;
}
int main(void) {
halInit();
chSysInit();
chThdCreateStatic(blinkWA, sizeof(blinkWA), NORMALPRIO, blink_thd, NULL);
/* set alarm in near future */
rtcGetTime(&RTCD1, ×pec);
alarmspec.tv_sec = timespec.tv_sec + 30;
rtcSetAlarm(&RTCD1, 0, &alarmspec);
while (TRUE){
chThdSleepSeconds(10);
chSysLock();
/* going to anabiosis*/
PWR->CR |= (PWR_CR_PDDS | PWR_CR_LPDS | PWR_CR_CSBF | PWR_CR_CWUF);
SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
__WFI();
}
return 0;
}
static THD_FUNCTION(can_reader_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("CAN reader");
chThdSleepMilliseconds(10);
can_rx_frame rx_msg;
mode_config_proto_t* proto = &con->mode->proto;
while (!chThdShouldTerminateX()) {
if(bsp_can_rxne(proto->dev_num)) {
chSysLock();
bsp_can_read(proto->dev_num, &rx_msg);
can_slcan_out(con, &rx_msg);
chSysUnlock();
} else {
chThdYield();
}
}
chThdExit((msg_t)1);
}
/**
* @brief Blocks the execution of the invoking thread until the specified
* thread terminates then the exit code is returned.
* @details This function waits for the specified thread to terminate then
* decrements its reference counter, if the counter reaches zero then
* the thread working area is returned to the proper allocator.<br>
* The memory used by the exited thread is handled in different ways
* depending on the API that spawned the thread:
* - If the thread was spawned by @p chThdCreateStatic() or by
* @p chThdInit() then nothing happens and the thread working area
* is not released or modified in any way. This is the default,
* totally static, behavior.
* - If the thread was spawned by @p chThdCreateFromHeap() then
* the working area is returned to the system heap.
* - If the thread was spawned by @p chThdCreateFromMemoryPool()
* then the working area is returned to the owning memory pool.
* .
* @pre The configuration option @p CH_USE_WAITEXIT must be enabled in
* order to use this function.
* @post Enabling @p chThdWait() requires 2-4 (depending on the
* architecture) extra bytes in the @p Thread structure.
* @post After invoking @p chThdWait() the thread pointer becomes invalid
* and must not be used as parameter for further system calls.
* @note If @p CH_USE_DYNAMIC is not specified this function just waits for
* the thread termination, no memory allocators are involved.
*
* @param[in] tp pointer to the thread
* @return The exit code from the terminated thread.
*
* @api
*/
msg_t chThdWait(Thread *tp) {
msg_t msg;
chDbgCheck(tp != NULL, "chThdWait");
chSysLock();
chDbgAssert(tp != currp, "chThdWait(), #1", "waiting self");
#if CH_USE_DYNAMIC
chDbgAssert(tp->p_refs > 0, "chThdWait(), #2", "not referenced");
#endif
if (tp->p_state != THD_STATE_FINAL) {
list_insert(currp, &tp->p_waiting);
chSchGoSleepS(THD_STATE_WTEXIT);
}
msg = tp->p_u.exitcode;
chSysUnlock();
#if CH_USE_DYNAMIC
chThdRelease(tp);
#endif
return msg;
}
/**
* @brief Shell termination handler.
*
* @param[in] id event id.
*/
static void termination_handler(eventid_t id) {
chThdSleepMilliseconds(10);
cputs("Init: shell on SD1 terminated");
chSysLock();
chOQResetI(&SD1.oqueue);
chSysUnlock();
// todo: 2nd port for TS
// if (shelltp2 && chThdTerminated(shelltp2)) {
// chThdWait(shelltp2);
// shelltp2 = NULL;
// chThdSleepMilliseconds(10);
// cputs("Init: shell on SD2 terminated");
// chSysLock();
// chOQResetI(&SD2.oqueue);
// chSysUnlock();
// }
}
/**
* @brief Transmits data via the I2C bus as master.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[in] txbuf pointer to the transmit buffer
* @param[in] txbytes number of bytes to be transmitted
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
i2cp->addr = addr;
i2cp->txbuf = txbuf;
i2cp->txbytes = txbytes;
i2cp->txidx = 0;
i2cp->rxbuf = rxbuf;
i2cp->rxbytes = rxbytes;
i2cp->rxidx = 0;
TWCR = ((1 << TWSTA) | (1 << TWINT) | (1 << TWEN) | (1 << TWIE));
chSysLock();
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
chSysUnlock();
return chThdSelf()->p_u.rdymsg;
}
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 msg_t l3g4200d_update_thread(void *p) {
SPIDriver *spip = (SPIDriver *)p;
while (TRUE) {
msg_t msg;
/* Waiting for the IRQ to happen.*/
chSysLock();
gyrotp = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
/* If data ready, update all axis.*/
if (msg == GYRO_DATA_READY) {
l3g4200d_update(spip);
}
}
return RDY_OK;
}
//-----------------------------------------------------------------------------
int
kuroBoxWriterStop(void)
{
chThdTerminate(loggerThread);
chThdTerminate(writerThread);
chThdWait(loggerThread);
// this thread may be in its own sleep
chSysLock();
if (writerThreadForSleep)
{
writerThreadForSleep->p_u.rdymsg = (msg_t)10; // just a random non-0
chSchReadyI(writerThreadForSleep);
writerThreadForSleep = NULL;
}
chSysUnlock();
chThdWait(writerThread);
return KB_OK;
}
/**
* @brief Brings the driver in a state safe for card removal.
*
* @param[in] mmcp pointer to the @p MMCDriver object
* @return The operation status.
*
* @retval CH_SUCCESS the operation succeeded and the driver is now
* in the @p MMC_INSERTED state.
* @retval CH_FAILED the operation failed.
*
* @api
*/
bool_t mmcDisconnect(MMCDriver *mmcp) {
chDbgCheck(mmcp != NULL, "mmcDisconnect");
chSysLock();
chDbgAssert((mmcp->state == BLK_ACTIVE) || (mmcp->state == BLK_READY),
"mmcDisconnect(), #1", "invalid state");
if (mmcp->state == BLK_ACTIVE) {
chSysUnlock();
return CH_SUCCESS;
}
mmcp->state = BLK_DISCONNECTING;
chSysUnlock();
/* Wait for the pending write operations to complete.*/
sync(mmcp);
spiStop(mmcp->config->spip);
mmcp->state = BLK_ACTIVE;
return CH_SUCCESS;
}
/**
* @brief Stops an ongoing conversion.
*
* @param[in] adcp pointer to the @p ADCDriver object
*/
void adcStopConversion(ADCDriver *adcp) {
chDbgCheck(adcp != NULL, "adcStopConversion");
chSysLock();
chDbgAssert((adcp->ad_state == ADC_READY) ||
(adcp->ad_state == ADC_RUNNING) ||
(adcp->ad_state == ADC_COMPLETE),
"adcStopConversion(), #1",
"invalid state");
if (adcp->ad_state == ADC_RUNNING) {
adc_lld_stop_conversion(adcp);
adcp->ad_grpp = NULL;
adcp->ad_state = ADC_READY;
chSemResetI(&adcp->ad_sem, 0);
chSchRescheduleS();
}
else
adcp->ad_state = ADC_READY;
chSysUnlock();
}
/**
* @brief Waits for all the specified events.
* @details The function waits for all the events specified in @p mask to
* become pending then the events are cleared and returned.
*
* @param[in] mask mask of the event flags that the function should wait
* for, @p ALL_EVENTS requires all the events
* @param[in] time the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_IMMEDIATE immediate timeout.
* - @a TIME_INFINITE no timeout.
* .
* @return The mask of the served and cleared events.
* @retval 0 if the operation has timed out.
*
* @api
*/
eventmask_t chEvtWaitAllTimeout(eventmask_t mask, systime_t time) {
Thread *ctp = currp;
chSysLock();
if ((ctp->p_epending & mask) != mask) {
if (TIME_IMMEDIATE == time) {
chSysUnlock();
return (eventmask_t)0;
}
ctp->p_u.ewmask = mask;
if (chSchGoSleepTimeoutS(THD_STATE_WTANDEVT, time) < RDY_OK) {
chSysUnlock();
return (eventmask_t)0;
}
}
ctp->p_epending &= ~mask;
chSysUnlock();
return mask;
}
void chVTSetAny(VirtualTimer *vtp, systime_t time, vtfunc_t vtfunc, void *par) {
if (isIsrContext()) {
chSysLockFromIsr()
;
if (chVTIsArmedI(vtp))
chVTResetI(vtp);
chVTSetI(vtp, time, vtfunc, par);
chSysUnlockFromIsr()
;
} else {
chSysLock()
;
if (chVTIsArmedI(vtp))
chVTResetI(vtp);
chVTSetI(vtp, time, vtfunc, par);
chSysUnlock()
;
}
}
/**
* @brief Changes the running thread priority level then reschedules if
* necessary.
* @note The function returns the real thread priority regardless of the
* current priority that could be higher than the real priority
* because the priority inheritance mechanism.
*
* @param[in] newprio the new priority level of the running thread
* @return The old priority level.
*
* @api
*/
tprio_t chThdSetPriority(tprio_t newprio) {
tprio_t oldprio;
chDbgCheck(newprio <= HIGHPRIO);
chSysLock();
#if CH_CFG_USE_MUTEXES == TRUE
oldprio = currp->realprio;
if ((currp->prio == currp->realprio) || (newprio > currp->prio)) {
currp->prio = newprio;
}
currp->realprio = newprio;
#else
oldprio = currp->prio;
currp->prio = newprio;
#endif
chSchRescheduleS();
chSysUnlock();
return oldprio;
}
void
vexLcdCheckReceiveMessage( LcdData *lcd )
{
int16_t i;
// any characters
if( sdGetWouldBlock(lcd->sdp) )
return;
// read up to 16 bytes from serial port
for(i=0;i<16;i++)
{
int16_t c;
c = sdGetTimeout( lcd->sdp, TIME_IMMEDIATE);
if( c != Q_TIMEOUT )
lcd->rxbuf[i] = c;
else
break;
}
// 6 chars ?
if( i == 6 )
{
// lcd message ?
if( (lcd->rxbuf[0] == 0xAA) && (lcd->rxbuf[1] == 0x55) && (lcd->rxbuf[2] == 0x16))
// verify checksum
if( !((lcd->rxbuf[4] + lcd->rxbuf[5]) & 0xFF) )
lcd->buttons = lcd->rxbuf[4];
}
// flush anything left
if( !sdGetWouldBlock(lcd->sdp) )
{
chSysLock();
chIQResetI( &(lcd->sdp)->iqueue );
chSysUnlock();
}
}
void lsm303_mag_update(SPIDriver *spip) {
uint8_t txbuf;
uint8_t rxbuf[6];
systime_t timestamp;
timestamp = chTimeNow();
txbuf = 0x80 | 0x40 | LSM303D_OUT_X_L_M;
spiAcquireBus(spip);
palClearPad(GPIOA, GPIOA_AM_CS);
spiSend(spip, 1, &txbuf);
spiReceive(spip, 6, &rxbuf);
palSetPad(GPIOA, GPIOA_AM_CS);
spiReleaseBus(spip);
chSysLock();
mag_data.t = timestamp;
mag_data.x = *((int16_t*)&(rxbuf[0])) >> 4;
mag_data.y = *((int16_t*)&(rxbuf[2])) >> 4;
mag_data.z = *((int16_t*)&(rxbuf[4])) >> 4;
chSysUnlock();
}
static void test_002_004_execute(void) {
tprio_t prio, p1;
/* [2.4.1] Simulating a priority boost situation (prio > realprio).*/
test_set_step(1);
{
prio = chThdGetPriorityX();
chThdGetSelfX()->prio += 2;
test_assert(chThdGetPriorityX() == prio + 2, "unexpected priority level");
}
/* [2.4.2] Raising thread priority above original priority but below
the boosted level.*/
test_set_step(2);
{
p1 = chThdSetPriority(prio + 1);
test_assert(p1 == prio, "unexpected returned priority level");
test_assert(chThdGetSelfX()->prio == prio + 2, "unexpected priority level");
test_assert(chThdGetSelfX()->realprio == prio + 1, "unexpected returned real priority level");
}
/* [2.4.3] Raising thread priority above the boosted level.*/
test_set_step(3);
{
p1 = chThdSetPriority(prio + 3);
test_assert(p1 == prio + 1, "unexpected returned priority level");
test_assert(chThdGetSelfX()->prio == prio + 3, "unexpected priority level");
test_assert(chThdGetSelfX()->realprio == prio + 3, "unexpected real priority level");
}
/* [2.4.4] Restoring original conditions.*/
test_set_step(4);
{
chSysLock();
chThdGetSelfX()->prio = prio;
chThdGetSelfX()->realprio = prio;
chSysUnlock();
}
}
static void bmk10_execute(void) {
static VirtualTimer vt1, vt2;
uint32_t n = 0;
test_wait_tick();
test_start_timer(1000);
do {
chSysLock();
chVTSetI(&vt1, 1, tmo, NULL);
chVTSetI(&vt2, 10000, tmo, NULL);
chVTResetI(&vt1);
chVTResetI(&vt2);
chSysUnlock();
n++;
#if defined(SIMULATOR)
ChkIntSources();
#endif
} while (!test_timer_done);
test_print("--- Score : ");
test_printn(n * 2);
test_println(" timers/S");
}
/**
* @brief Releases a reference to a thread object.
* @details If the references counter reaches zero <b>and</b> the thread
* is in the @p CH_STATE_FINAL state then the thread's memory is
* returned to the proper allocator.
* @pre The configuration option @p CH_CFG_USE_DYNAMIC must be enabled in
* order to use this function.
* @note Static threads are not affected.
*
* @param[in] tp pointer to the thread
*
* @api
*/
void chThdRelease(thread_t *tp) {
trefs_t refs;
chSysLock();
chDbgAssert(tp->p_refs > (trefs_t)0, "not referenced");
tp->p_refs--;
refs = tp->p_refs;
/* If the references counter reaches zero and the thread is in its
terminated state then the memory can be returned to the proper
allocator. Of course static threads are not affected.*/
if ((refs == (trefs_t)0) && (tp->p_state == CH_STATE_FINAL)) {
switch (tp->p_flags & CH_FLAG_MODE_MASK) {
#if CH_CFG_USE_HEAP == TRUE
case CH_FLAG_MODE_HEAP:
#if CH_CFG_USE_REGISTRY == TRUE
REG_REMOVE(tp);
#endif
chSysUnlock();
chHeapFree(tp);
return;
#endif
#if CH_CFG_USE_MEMPOOLS == TRUE
case CH_FLAG_MODE_MPOOL:
#if CH_CFG_USE_REGISTRY == TRUE
REG_REMOVE(tp);
#endif
chSysUnlock();
chPoolFree(tp->p_mpool, tp);
return;
#endif
default:
/* Nothing to do for static threads, those are removed from the
registry on exit.*/
break;
}
}
chSysUnlock();
}
void statusLedPulse(int led, systime_t duration)
{
chSysLock();
switch(led)
{
case 1:
if (chVTIsArmedI(&vt1))
chVTResetI(&vt1);
else
palSetPad(IOPORT2, GPIOB_LED1);
chVTSetI(&vt1, duration, ledoff_1, NULL);
break;
case 2:
if (chVTIsArmedI(&vt2))
chVTResetI(&vt2);
else
palSetPad(IOPORT2, GPIOB_LED2);
chVTSetI(&vt2, duration, ledoff_2, NULL);
break;
}
chSysUnlock();
}
/**
* @brief Receives data via the I2C bus as master.
* @details Number of receiving bytes must be more than 1 on STM32F1x. This is
* hardware restriction.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
LPC_I2C_TypeDef *dp = i2cp->i2c;
VirtualTimer vt;
i2cp->addr = addr << 1;
/* Global timeout for the whole operation.*/
if (timeout != TIME_INFINITE)
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
/* Releases the lock from high level driver.*/
chSysUnlock();
/* Initializes driver fields */
i2cp->errors = 0;
i2cp->rxbuf = rxbuf;
i2cp->rxbytes = rxbytes;
/* This lock will be released in high level driver.*/
chSysLock();
/* Atomic check on the timer in order to make sure that a timeout didn't
happen outside the critical zone.*/
if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
return RDY_TIMEOUT;
/* Starts the operation.*/
dp->CONSET = I2C_CONSET_STA;
/* Waits for the operation completion or a timeout.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
chVTResetI(&vt);
return chThdSelf()->p_u.rdymsg;
}
static msg_t initUsbTransfer(void *arg) {
(void)arg;
uint8_t i;
chRegSetThreadName("initUsbTransfer");
while (TRUE) {
//wait until enough ADC data is aquired or an overflow is detected
while((((p1+BUFFLEN-p2)%BUFFLEN)<IN_PACKETSIZE) && !overflow){
chThdSleepMilliseconds(1);
}
// copy the ADC data to the USB transfer buffer
for (i=0;i<IN_PACKETSIZE;i++){
transferBuf[i]=((uint8_t*) data)[p2];
//if an overflow is detected, write the overflow count.
// in a real application this has to be send on another channel
if(overflow){
transferBuf[i]= overflow;
overflow=0;
}
p2 = (p2+1)%BUFFLEN;
}
//wait for the last transmission to complete
while(transmitting){
chThdSleepMilliseconds(1);
}
transmitting = 1;
usbPrepareTransmit(usbp, EP_IN, transferBuf, IN_PACKETSIZE);
chSysLock();
usbStartTransmitI(usbp, EP_IN);
chSysUnlock();
}
return 0;
}
/**
* @brief Creates a new thread into a static memory area.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
*
* @param[out] wsp pointer to a working area dedicated to the thread stack
* @param[in] size size of the working area
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
*
* @api
*/
thread_t *chThdCreateStatic(void *wsp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
thread_t *tp;
chDbgCheck((wsp != NULL) &&
MEM_IS_ALIGNED(wsp, PORT_WORKING_AREA_ALIGN) &&
(size >= THD_WORKING_AREA_SIZE(0)) &&
MEM_IS_ALIGNED(size, PORT_STACK_ALIGN) &&
(prio <= HIGHPRIO) && (pf != NULL));
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
/* The thread structure is laid out in the upper part of the thread
workspace. The thread position structure is aligned to the required
stack alignment because it represents the stack top.*/
tp = (thread_t *)((uint8_t *)wsp + size -
MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN));
/* Stack boundary.*/
tp->stklimit = (stkalign_t *)wsp;
/* Setting up the port-dependent part of the working area.*/
PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg);
tp = _thread_init(tp, "noname", prio);
/* Starting the thread immediately.*/
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
