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);
}
