static void rt_test_005_006_execute(void) { binary_semaphore_t bsem; msg_t msg; /* [5.6.1] Creating a binary semaphore in "taken" state, the state is checked.*/ test_set_step(1); { chBSemObjectInit(&bsem, true); test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken"); } /* [5.6.2] Resetting the binary semaphore in "taken" state, the state must not change.*/ test_set_step(2); { chBSemReset(&bsem, true); test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken"); } /* [5.6.3] Starting a signaler thread at a lower priority.*/ test_set_step(3); { threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-1, thread4, &bsem); } /* [5.6.4] Waiting for the binary semaphore to be signaled, the semaphore is expected to be taken.*/ test_set_step(4); { msg = chBSemWait(&bsem); test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken"); test_assert(msg == MSG_OK, "unexpected message"); } /* [5.6.5] Signaling the binary semaphore, checking the binary semaphore state to be "not taken" and the underlying counter semaphore counter to be one.*/ test_set_step(5); { chBSemSignal(&bsem); test_assert_lock(chBSemGetStateI(&bsem) ==false, "still taken"); test_assert_lock(chSemGetCounterI(&bsem.sem) == 1, "unexpected counter"); } /* [5.6.6] Signaling the binary semaphore again, the internal state must not change from "not taken".*/ test_set_step(6); { chBSemSignal(&bsem); test_assert_lock(chBSemGetStateI(&bsem) == false, "taken"); test_assert_lock(chSemGetCounterI(&bsem.sem) == 1, "unexpected counter"); } }
void motor_driver_update_trajectory(motor_driver_t *d, trajectory_chunk_t *traj) { chBSemWait(&d->lock); if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) { d->setpt.trajectory = chPoolAlloc(d->traj_buffer_pool); float *traj_mem = chPoolAlloc(d->traj_buffer_points_pool); if (d->setpt.trajectory == NULL || traj_mem == NULL) { chSysHalt("motor driver out of memory (trajectory buffer allocation)"); } trajectory_init(d->setpt.trajectory, traj_mem, d->traj_buffer_nb_points, 4, traj->sampling_time_us); d->control_mode = MOTOR_CONTROL_MODE_TRAJECTORY; } int ret = trajectory_apply_chunk(d->setpt.trajectory, traj); switch (ret) { case TRAJECTORY_ERROR_TIMESTEP_MISMATCH: chSysHalt("TRAJECTORY_ERROR_TIMESTEP_MISMATCH"); break; case TRAJECTORY_ERROR_CHUNK_TOO_OLD: chSysHalt("TRAJECTORY_ERROR_CHUNK_TOO_OLD"); break; case TRAJECTORY_ERROR_DIMENSION_MISMATCH: chSysHalt("TRAJECTORY_ERROR_DIMENSION_MISMATCH"); break; case TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER: log_message("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER"); // chSysHalt("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER"); break; } d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TRAJECTORY; chBSemSignal(&d->lock); }
/** Release claimed USART. * \see ::usart_claim * \param s The USART DMA state structure. */ void usart_release(usart_state* s) { if (s->claim_nest) s->claim_nest--; else chBSemSignal(&s->claimed); }
void io_dev_send(const void *dtgrm, size_t len, void *arg) { struct io_dev_s *dev = (struct io_dev_s *)arg; chBSemWait(&dev->lock); serial_datagram_send(dtgrm, len, send_cb, dev->channel); chBSemSignal(&dev->lock); }
uint8_t i2c_t::Write(uint32_t Addr, uint8_t *WPtr, uint32_t WLength) { if(chBSemWait(&BSemaphore) != MSG_OK) return FAILURE; uint8_t Rslt; msg_t r; I2C_TypeDef *pi2c = PParams->pi2c; // To make things shorter if(WLength == 0 or WPtr == nullptr) { Rslt = CMD_ERROR; goto WriteEnd; } if(IBusyWait() != OK) { Rslt = BUSY; goto WriteEnd; } IReset(); // Reset I2C // Prepare TX DMA dmaStreamSetMode(PParams->PDmaTx, DMA_MODE_TX); dmaStreamSetMemory0(PParams->PDmaTx, WPtr); dmaStreamSetTransactionSize(PParams->PDmaTx, WLength); // Prepare tx IState = istWrite; // Nothing to read pi2c->CR2 = (Addr << 1) | (WLength << 16); dmaStreamEnable(PParams->PDmaTx); // Enable TX DMA // Enable IRQs: TX completed, error, NAck pi2c->CR1 |= (I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE); pi2c->CR2 |= I2C_CR2_START; // Start transmission // Wait completion chSysLock(); r = chThdSuspendTimeoutS(&PThd, MS2ST(I2C_TIMEOUT_MS)); chSysUnlock(); // Disable IRQs pi2c->CR1 &= ~(I2C_CR1_TCIE | I2C_CR1_ERRIE | I2C_CR1_NACKIE); if(r == MSG_TIMEOUT) { pi2c->CR2 |= I2C_CR2_STOP; Rslt = TIMEOUT; } else Rslt = (IState == istFailure)? FAILURE : OK; WriteEnd: chBSemSignal(&BSemaphore); return Rslt; }
void i2c_t::ScanBus() { if(chBSemWait(&BSemaphore) != MSG_OK) return; Uart.Printf(" 0 1 2 3 4 5 6 7 8 9 a b c d e f"); uint32_t AddrHi, Addr; I2C_TypeDef *pi2c = PParams->pi2c; // To make things shorter for(AddrHi = 0; AddrHi < 0x80; AddrHi += 0x10) { Uart.Printf("\r%02X: ", AddrHi); for(uint32_t n=0; n<0x10; n++) { Addr = AddrHi + n; if(Addr <= 0x01 or Addr > 0x77) Uart.Printf(" "); else { IReset(); // Reset I2C // Set addr and autoend; NBYTES = 0 pi2c->CR2 = (Addr << 1) | I2C_CR2_AUTOEND; pi2c->CR2 |= I2C_CR2_START; // Start while(!(pi2c->ISR & I2C_ISR_STOPF)); if(pi2c->ISR & I2C_ISR_NACKF) Uart.Printf("__ "); else Uart.Printf("%02X ", Addr); } } // for lo } // for hi // Disable I2C pi2c->CR1 &= ~I2C_CR1_PE; Uart.Printf("\r"); chBSemSignal(&BSemaphore); }
void motor_driver_disable(motor_driver_t *d) { chBSemWait(&d->lock); free_trajectory_buffer(d); d->control_mode = MOTOR_CONTROL_MODE_DISABLED; chBSemSignal(&d->lock); }
uint8_t Pill_t::CheckIfConnected() { if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE; uint8_t Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, NULL, 0, NULL, 0); Connected = (Rslt == OK); if(i2c.Error == true) PillReset(); chBSemSignal(&ISem); return Rslt; }
void motor_driver_set_voltage(motor_driver_t *d, float voltage) { chBSemWait(&d->lock); free_trajectory_buffer(d); d->control_mode = MOTOR_CONTROL_MODE_VOLTAGE; d->setpt.voltage = voltage; d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_VOLTAGE; chBSemSignal(&d->lock); }
void motor_driver_set_torque(motor_driver_t *d, float torque) { chBSemWait(&d->lock); free_trajectory_buffer(d); d->control_mode = MOTOR_CONTROL_MODE_TORQUE; d->setpt.torque = torque; d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TORQUE; chBSemSignal(&d->lock); }
void motor_driver_set_velocity(motor_driver_t *d, float velocity) { chBSemWait(&d->lock); free_trajectory_buffer(d); d->control_mode = MOTOR_CONTROL_MODE_VELOCITY; d->setpt.velocity = velocity; d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_VELOCITY; chBSemSignal(&d->lock); }
void motor_driver_set_position(motor_driver_t *d, float position) { chBSemWait(&d->lock); free_trajectory_buffer(d); d->control_mode = MOTOR_CONTROL_MODE_POSITION; d->setpt.position = position; d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_POSITION; chBSemSignal(&d->lock); }
uint8_t Pill_t::Read(uint8_t *Ptr, uint8_t Length) { if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE; uint8_t WordAddress = PILL_START_ADDR; uint8_t Rslt = i2c.CmdWriteRead(EEADDR+IAddr, &WordAddress, 1, Ptr, Length); Connected = (Rslt == OK); if(i2c.Error == true) PillReset(); chBSemSignal(&ISem); return Rslt; }
/** Drive all SPI nCS lines high. * Should be called after an SPI transfer is finished. */ void spi_slave_deselect(void) { /* Deselect FPGA CS */ gpio_set(GPIOA, GPIO4); /* Deselect configuration flash and front-end CS */ gpio_set(GPIOB, GPIO11 | GPIO12); chBSemSignal(&spi_sem); }
void StartReading(void) { if (FlashSate == FLASH_IDLE) { DisableMPUInt(); FlashSate = FLASH_READING; OpCode = READING_OPCODE; chBSemSignal(&FlashSemaphore); } }
void StartRecording(void) { if (FlashSate == FLASH_IDLE) { FlashSate = FLASH_ERASING; OpCode = RECORDING_OPCODE; chBSemSignal(&FlashSemaphore); } }
void gtimerJab(GTimer *pt) { chMtxLock(&mutex); // Jab it! pt->flags |= GTIMER_FLG_JABBED; // Bump the thread chBSemSignal(&waitsem); chMtxUnlock(); }
void WritePage(uint8_t *Buffer) { if (FlashSate == FLASH_WAIT_FOR_PAGE) { FlashSate = FLASH_WRITING; OpCode = WRITEPAGE_OPCODE; DataBuffer = Buffer; chBSemSignal(&FlashSemaphore); } else if (FlashSate != FLASH_FINISHED) TURN_O_LED_ON(); }
void geventDetachSource(GListener *pl, GSourceHandle gsh) { if (pl) { chMtxLock(&geventMutex); deleteAssignments(pl, gsh); if (!gsh && chSemGetCounterI(&pl->waitqueue) < 0) { chBSemWait(&pl->eventlock); // Obtain the buffer lock pl->event.type = GEVENT_EXIT; // Set up the EXIT event chSemSignal(&pl->waitqueue); // Wake up the listener chBSemSignal(&pl->eventlock); // Release the buffer lock } chMtxUnlock(); } }
/** * @brief EEPROM read routine. * * @param[in] addr addres of 1-st byte to be read * @param[in] len number of bytes to be write * @param[in] ext_rxbuf pointer to data buffer */ msg_t eeprom_read(uint32_t addr, uint8_t *buf, size_t len){ msg_t status = RDY_OK; chBSemWait(&eeprom_sem); chDbgCheck(((len <= EEPROM_SIZE) && ((addr+len) <= EEPROM_SIZE)), "requested data out of device bounds"); eeprom_split_addr(localtxbuf, addr); /* write address bytes */ i2c_transmit(eeprom_i2caddr, localtxbuf, 2, buf, len); chBSemSignal(&eeprom_sem); return status; }
void geventRegisterCallback(GListener *pl, GEventCallbackFn fn, void *param) { if (pl) { chMtxLock(&geventMutex); chBSemWait(&pl->eventlock); // Obtain the buffer lock pl->param = param; // Set the param pl->callback = fn; // Set the callback function if (chSemGetCounterI(&pl->waitqueue) < 0) { pl->event.type = GEVENT_EXIT; // Set up the EXIT event chSemSignal(&pl->waitqueue); // Wake up the listener } chBSemSignal(&pl->eventlock); // Release the buffer lock chMtxUnlock(); } }
static msg_t PollMPU6050Thread(void *arg) { systime_t time; (void)arg; time = chTimeNow(); while (TRUE) { if (mpu6050GetNewData()) { chBSemSignal(&bsemNewDataReady); } /* Wait until the next 2 milliseconds passes. */ chThdSleepUntil(time += MS2ST(2)); } /* This point should never be reached. */ return 0; }
/* Null is treated as a wildcard. */ static void deleteAssignments(GListener *pl, GSourceHandle gsh) { GSourceListener *psl; for(psl = Assignments; psl < Assignments+GEVENT_MAX_SOURCE_LISTENERS; psl++) { if ((!pl || psl->pListener == pl) && (!gsh || psl->pSource == gsh)) { if (chSemGetCounterI(&psl->pListener->waitqueue) < 0) { chBSemWait(&psl->pListener->eventlock); // Obtain the buffer lock psl->pListener->event.type = GEVENT_EXIT; // Set up the EXIT event chSemSignal(&psl->pListener->waitqueue); // Wake up the listener chBSemSignal(&psl->pListener->eventlock); // Release the buffer lock } psl->pListener = 0; } } }
void gtimerStart(GTimer *pt, GTimerFunction fn, void *param, bool_t periodic, systime_t millisec) { chMtxLock(&mutex); // Start our thread if not already going if (!pThread) pThread = chThdCreateStatic(waTimerThread, sizeof(waTimerThread), HIGHPRIO, GTimerThreadHandler, NULL); // Is this already scheduled? if (pt->flags & GTIMER_FLG_SCHEDULED) { // Cancel it! if (pt->next == pt->prev) pTimerHead = 0; else { pt->next->prev = pt->prev; pt->prev->next = pt->next; if (pTimerHead == pt) pTimerHead = pt->next; } } // Set up the timer structure pt->fn = fn; pt->param = param; pt->flags = GTIMER_FLG_SCHEDULED; if (periodic) pt->flags |= GTIMER_FLG_PERIODIC; if (millisec == TIME_INFINITE) { pt->flags |= GTIMER_FLG_INFINITE; pt->period = TIME_INFINITE; } else { pt->period = MS2ST(millisec); pt->when = chTimeNow() + pt->period; } // Just pop it on the end of the queue if (pTimerHead) { pt->next = pTimerHead; pt->prev = pTimerHead->prev; pt->prev->next = pt; pt->next->prev = pt; } else pt->next = pt->prev = pTimerHead = pt; // Bump the thread if (!(pt->flags & GTIMER_FLG_INFINITE)) chBSemSignal(&waitsem); chMtxUnlock(); }
/** * @brief EEPROM write routine. * @details Function writes data to EEPROM. * @pre Data must be fit to single EEPROM page. * * @param[in] addr addres of 1-st byte to be write * @param[in] buf pointer to data * @param[in] len number of bytes to be written */ msg_t eeprom_write(uint32_t addr, const uint8_t *buf, size_t len){ msg_t status = RDY_OK; chBSemWait(&eeprom_sem); chDbgCheck(((len <= EEPROM_SIZE) && ((addr+len) <= EEPROM_SIZE)), "data can not be fitted in device"); chDbgCheck(((addr / EEPROM_PAGE_SIZE) == ((addr + len - 1) / EEPROM_PAGE_SIZE)), "data can not be fitted in single page"); eeprom_split_addr(localtxbuf, addr); /* write address bytes */ memcpy(&(localtxbuf[2]), buf, len); /* write data bytes */ i2c_transmit(eeprom_i2caddr, localtxbuf, (len + 2), NULL, 0); /* wait until EEPROM process data */ chThdSleepMilliseconds(EEPROM_WRITE_TIME); chBSemSignal(&eeprom_sem); return status; }
static void usb_rx(uint8_t c) { static int framebuf_ctr = 0; if(framebuf_ctr < 6) { /* Wait for sync packet */ if(c != 0xFF) { framebuf_ctr = 0; } else { framebuf.sync[framebuf_ctr++] = c; } } else { /* Process new actual data */ framebuf.raw[framebuf_ctr++] = c; if(framebuf_ctr == sizeof(framebuf)) { framebuf_ctr = 0; chBSemSignal(&frame_thread_sem); } } }
uint8_t Pill_t::Write(uint8_t *Ptr, uint8_t Length) { if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE; uint8_t WordAddress = PILL_START_ADDR; uint8_t Rslt = OK; // Write page by page while(Length) { uint8_t ToWriteCnt = (Length > PILL_PAGE_SZ)? PILL_PAGE_SZ : Length; Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, &WordAddress, 1, Ptr, ToWriteCnt); if(Rslt == OK) { chThdSleepMilliseconds(5); // Allow memory to complete writing Length -= ToWriteCnt; Ptr += ToWriteCnt; WordAddress += ToWriteCnt; } else break; } Connected = (Rslt == OK); if(i2c.Error == true) PillReset(); chBSemSignal(&ISem); return Rslt; }
bool_t geventAttachSource(GListener *pl, GSourceHandle gsh, unsigned flags) { GSourceListener *psl, *pslfree; // Safety first if (!pl || !gsh) { GEVENT_ASSERT(FALSE); return FALSE; } chMtxLock(&geventMutex); // Check if this pair is already in the table (scan for a free slot at the same time) pslfree = 0; for(psl = Assignments; psl < Assignments+GEVENT_MAX_SOURCE_LISTENERS; psl++) { if (pl == psl->pListener && gsh == psl->pSource) { // Just update the flags chBSemWait(&pl->eventlock); // Safety first - just in case a source is using it psl->listenflags = flags; chBSemSignal(&pl->eventlock); // Release this lock chMtxUnlock(); return TRUE; } if (!pslfree && !psl->pListener) pslfree = psl; } // A free slot was found - allocate it if (pslfree) { pslfree->pListener = pl; pslfree->pSource = gsh; pslfree->listenflags = flags; pslfree->srcflags = 0; } chMtxUnlock(); GEVENT_ASSERT(pslfree != 0); return pslfree != 0; }
GSourceListener *geventGetSourceListener(GSourceHandle gsh, GSourceListener *lastlr) { GSourceListener *psl; // Safety first if (!gsh) return 0; chMtxLock(&geventMutex); // Unlock the last listener event buffer if (lastlr) chBSemSignal(&lastlr->pListener->eventlock); // Loop through the table looking for attachments to this source for(psl = lastlr ? (lastlr+1) : Assignments; psl < Assignments+GEVENT_MAX_SOURCE_LISTENERS; psl++) { if (gsh == psl->pSource) { chBSemWait(&psl->pListener->eventlock); // Obtain a lock on the listener event buffer chMtxUnlock(); return psl; } } chMtxUnlock(); return 0; }
static THD_FUNCTION(thread4, p) { chBSemSignal((binary_semaphore_t *)p); }