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);
}
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 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);
}
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);
}
static THD_FUNCTION(frame_thread, arg) {
(void)arg;
while(true) {
chBSemWait(&frame_thread_sem);
frame_process();
}
}
void spi1_xfer_dma(u16 n_bytes, u8 data_in[], const u8 data_out[])
{
/* We use a static buffer here for DMA transfers as data_in/data_out
* often are on the stack in CCM which is not accessible by DMA.
*/
static volatile u8 spi_dma_buf[128];
memcpy((u8*)spi_dma_buf, data_out, n_bytes);
/* Setup transmit stream */
DMA_SM0AR(DMA2, 3) = spi_dma_buf;
DMA_SNDTR(DMA2, 3) = n_bytes;
/* Setup receive stream */
DMA_SM0AR(DMA2, 0) = spi_dma_buf;
DMA_SNDTR(DMA2, 0) = n_bytes;
/* We need a memory buffer here to avoid a transfer error */
asm volatile ("dmb");
/* Enable the DMA RX channel. */
DMA_SCR(DMA2, 0) |= DMA_SxCR_EN;
/* Enable the transmit channel to begin the transaction */
DMA_SCR(DMA2, 3) |= DMA_SxCR_EN;
/* Yeild the CPU while we wait for the transaction to complete */
chBSemWait(&spi_dma_sem);
if (data_in != NULL)
memcpy(data_in, (u8*)spi_dma_buf, n_bytes);
}
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);
}
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_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_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);
}
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");
}
}
static msg_t AttitudeThread(void *arg) {
(void)arg;
attitudeInit();
while (TRUE) {
if (chBSemWait(&bsemNewDataReady) == RDY_OK) {
attitudeUpdate();
actuatorsUpdate();
}
}
/* This point should never be reached. */
return 0;
}
void motor_right_on(Direction dir){
chBSemWait(&motor_right_sem);
chSysLock();
if(dir == D_FORWARD){
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,motor_speed));
}else{
pwmEnableChannelI(motor_pwm, rp1, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,motor_speed));
pwmEnableChannelI(motor_pwm, rp2, PWM_PERCENTAGE_TO_WIDTH(motor_pwm,0));
}
chSysUnlock();
}
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();
}
}
static msg_t execThread( void *arg )
{
(void)arg;
chRegSetThreadName( "e" );
while ( 1 )
{
chBSemWait( &semaphore );
// If's come here run Pawn machine.
pawnExec( &g_pawn );
}
return 0;
}
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();
}
}
/**
* @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;
}
/* 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 bus_voltage_adc_conversion(void)
{
// read bus voltage
adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, ADC_BUF_DEPTH);
chBSemWait(&adc_wait);
// set LED
float voltage = bus_voltage_get();
if (voltage > 4.5f && voltage < 5.5f) {
led_set(CAN1_PWR_LED);
} else if (voltage > 0.5f || voltage > 5.5f) {
// voltage warning
led_toggle(CAN1_PWR_LED);
} else {
led_clear(CAN1_PWR_LED);
}
}
/** Drive SPI nCS line low for selected peripheral.
* \param slave Peripheral to drive chip select for.
*/
void spi_slave_select(u8 slave)
{
chBSemWait(&spi_sem);
switch (slave) {
case SPI_SLAVE_FPGA:
gpio_clear(GPIOA, GPIO4);
break;
case SPI_SLAVE_FLASH:
gpio_clear(GPIOB, GPIO12);
break;
case SPI_SLAVE_FRONTEND:
gpio_clear(GPIOB, GPIO11);
break;
}
}
/**
* @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;
}
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;
}
void gadcLowSpeedGet(uint32_t physdev, adcsample_t *buffer) {
struct lsdev *p;
BSEMAPHORE_DECL(mysem, TRUE);
/* Start the Low Speed Timer */
chMtxLock(&gadcmutex);
if (!gtimerIsActive(&LowSpeedGTimer))
gtimerStart(&LowSpeedGTimer, LowSpeedGTimerCallback, NULL, TRUE, TIME_INFINITE);
chMtxUnlock();
while(1) {
/* Wait for an available slot */
chSemWait(&gadcsem);
/* Find a slot */
chMtxLock(&gadcmutex);
for(p = ls; p < &ls[GADC_MAX_LOWSPEED_DEVICES]; p++) {
if (!(p->flags & GADC_FLG_ISACTIVE)) {
p->lld.physdev = physdev;
p->lld.buffer = buffer;
p->fn = BSemSignalCallback;
p->param = &mysem;
p->flags = GADC_FLG_ISACTIVE;
chMtxUnlock();
StartADC(FALSE);
chBSemWait(&mysem);
return;
}
}
chMtxUnlock();
/**
* We should never get here - the count semaphore must be wrong.
* Decrement it and try again.
*/
}
}
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;
}
msg_t BinarySemaphore::wait(void) {
return chBSemWait(&bsem);
}
void motor_driver_lock(motor_driver_t *d)
{
chBSemWait(&d->lock);
}
void sc_lsm9ds0_read(sc_float *acc, sc_float *magn, sc_float *gyro)
{
uint8_t txbuf[1];
uint8_t rxbuf[6];
uint8_t sensors_done = 0;
uint8_t i;
const uint8_t all_done = SENSOR_RDY_ACC | SENSOR_RDY_MAGN | SENSOR_RDY_GYRO;
// Read all sensors at least once
while (sensors_done != all_done){
// Wait for data ready signal
chBSemWait(&lsm9ds0_drdy_sem);
#ifdef SC_WAR_ISSUE_1
// WAR for broken DRDY_G pin
if (sensors_ready & SENSOR_RDY_ACC) {
chMtxLock(&data_mtx);
sensors_ready |= SENSOR_RDY_GYRO;
chMtxUnlock();
}
#endif
while (sensors_ready && sensors_done != all_done) {
if (sensors_ready & SENSOR_RDY_ACC) {
txbuf[0] = LSM9DS0_OUT_X_L_A | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_xm, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
acc[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_ACC_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_ACC;
chMtxUnlock();
sensors_done |= SENSOR_RDY_ACC;
}
if (sensors_ready & SENSOR_RDY_MAGN) {
txbuf[0] = LSM9DS0_OUT_X_L_M | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_xm, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
magn[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_MAGN_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_MAGN;
chMtxUnlock();
sensors_done |= SENSOR_RDY_MAGN;
}
if (sensors_ready & SENSOR_RDY_GYRO) {
txbuf[0] = LSM9DS0_OUT_X_L_G | LSM9DS0_SUBADDR_AUTO_INC_BIT;
sc_i2c_transmit(i2cn_g, txbuf, 1, rxbuf, 6);
for (i = 0; i < 3; ++i) {
gyro[i] = ((int16_t)(((uint16_t)rxbuf[2*i + 1]) << 8 | rxbuf[2*i])) *
LSM9DS0_GYRO_SENSITIVITY;
}
chMtxLock(&data_mtx);
sensors_ready &= ~SENSOR_RDY_GYRO;
chMtxUnlock();
sensors_done |= SENSOR_RDY_GYRO;
}
}
}
}
/************************************************************************
* NAME: fnet_os_event_wait
*
* DESCRIPTION:
*************************************************************************/
void fnet_os_event_wait(void)
{
chBSemWait(&FnetSemaphore);
}
uint32_t dma_storm_spi_stop(void){
stop = true;
chBSemWait(&sem);
spiStop(&SPID1);
return its;
}
