void dma_storm_spi_start(void){
its = 0;
stop = false;
chBSemObjectInit(&sem, true);
spiStart(&SPID1, &spicfg);
spiStartExchange(&SPID1, SPI_BUF_SIZE, testbuf_flash, testbuf_ram);
}
int main(void)
{
halInit();
chSysInit();
chBSemObjectInit(&frame_thread_sem, false);
bus_init();
power_init();
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
chThdCreateStatic(frame_thread_wa, sizeof(frame_thread_wa),
NORMALPRIO, frame_thread, NULL);
while(true) {
usb_rx(streamGet(&SDU1));
}
}
void i2c_t::Init() {
// GPIO
PinSetupAlterFunc(PParams->PGpio, PParams->SclPin, omOpenDrain, pudNone, PParams->PinAF);
PinSetupAlterFunc(PParams->PGpio, PParams->SdaPin, omOpenDrain, pudNone, PParams->PinAF);
#if I2C_USE_SEMAPHORE
chBSemObjectInit(&BSemaphore, NOT_TAKEN);
#endif
// I2C
I2C_TypeDef *pi2c = PParams->pi2c; // To make things shorter
pi2c->CR1 = 0; // Clear PE bit => disable and reset i2c
if(pi2c == I2C1) {
rccResetI2C1();
rccEnableI2C1(FALSE);
}
else if(pi2c == I2C2) {
rccResetI2C2();
rccEnableI2C2(FALSE);
}
else if(pi2c == I2C3) {
rccResetI2C3();
rccEnableI2C3(FALSE);
}
pi2c->TIMINGR = PParams->Timing; // setup timings
// Analog filter enabled, digital disabled, clk stretch enabled, DMA enabled
pi2c->CR1 = I2C_CR1_TXDMAEN | I2C_CR1_RXDMAEN;
// DMA
dmaStreamAllocate(STM32_DMA1_STREAM2, IRQ_PRIO_MEDIUM, nullptr, nullptr);
dmaStreamAllocate(STM32_DMA1_STREAM3, IRQ_PRIO_MEDIUM, nullptr, nullptr);
dmaStreamSetPeripheral(PParams->PDmaRx, &pi2c->RXDR);
dmaStreamSetPeripheral(STM32_DMA1_STREAM2, &pi2c->TXDR);
// IRQ
nvicEnableVector(72, IRQ_PRIO_MEDIUM);
nvicEnableVector(73, IRQ_PRIO_MEDIUM);
}
/**
* @brief Create a mutex.
* @note @p mutex_def is not used.
* @note Can involve memory allocation.
*/
osMutexId osMutexCreate(const osMutexDef_t *mutex_def) {
(void)mutex_def;
binary_semaphore_t *mtx = chPoolAlloc(&sempool);
chBSemObjectInit(mtx, false);
return mtx;
}
void motor_driver_init(motor_driver_t *d,
const char *actuator_id,
parameter_namespace_t *ns,
memory_pool_t *traj_buffer_pool,
memory_pool_t *traj_buffer_points_pool,
int traj_buffer_nb_points)
{
chBSemObjectInit(&d->lock, false);
d->traj_buffer_pool = traj_buffer_pool;
d->traj_buffer_points_pool = traj_buffer_points_pool;
d->traj_buffer_nb_points = traj_buffer_nb_points;
strncpy(d->id, actuator_id, MOTOR_ID_MAX_LEN);
d->id[MOTOR_ID_MAX_LEN] = '\0';
d->can_id = CAN_ID_NOT_SET;
d->control_mode = MOTOR_CONTROL_MODE_DISABLED;
d->update_period = 1;
d->can_driver = NULL;
parameter_namespace_declare(&d->config.root, ns, d->id);
parameter_namespace_declare(&d->config.control, &d->config.root, "control");
pid_register(&d->config.position_pid, &d->config.control, "position");
pid_register(&d->config.velocity_pid, &d->config.control, "velocity");
pid_register(&d->config.current_pid, &d->config.control, "current");
parameter_scalar_declare_with_default(&d->config.torque_limit, &d->config.control, "torque_limit", 0);
parameter_scalar_declare_with_default(&d->config.velocity_limit, &d->config.control, "velocity_limit", 0);
parameter_scalar_declare_with_default(&d->config.acceleration_limit, &d->config.control, "acceleration_limit", 0);
parameter_scalar_declare_with_default(&d->config.low_batt_th, &d->config.control, "low_batt_th", 12);
parameter_namespace_declare(&d->config.thermal, &d->config.root, "thermal");
parameter_scalar_declare(&d->config.thermal_capacity, &d->config.thermal, "capacity");
parameter_scalar_declare(&d->config.thermal_resistance, &d->config.thermal, "resistance");
parameter_scalar_declare(&d->config.thermal_current_gain, &d->config.thermal, "current_gain");
parameter_scalar_declare(&d->config.max_temperature, &d->config.thermal, "max_temperature");
parameter_namespace_declare(&d->config.motor, &d->config.root, "motor");
parameter_scalar_declare_with_default(&d->config.torque_constant, &d->config.motor, "torque_constant", 1);
parameter_integer_declare_with_default(&d->config.transmission_ratio_p, &d->config.motor, "transmission_ratio_p", 1);
parameter_integer_declare_with_default(&d->config.transmission_ratio_q, &d->config.motor, "transmission_ratio_q", 1);
parameter_integer_declare_with_default(&d->config.motor_encoder_steps_per_revolution, &d->config.motor, "motor_encoder_steps_per_revolution", 4096);
parameter_integer_declare_with_default(&d->config.second_encoder_steps_per_revolution, &d->config.motor, "second_encoder_steps_per_revolution", 4096);
parameter_scalar_declare_with_default(&d->config.potentiometer_gain, &d->config.motor, "potentiometer_gain", 1);
parameter_integer_declare_with_default(&d->config.mode, &d->config.motor, "mode", 4); // todo
parameter_namespace_declare(&d->config.stream, &d->config.root, "stream");
parameter_scalar_declare(&d->config.current_pid_stream, &d->config.stream, "current_pid");
parameter_scalar_declare(&d->config.velocity_pid_stream, &d->config.stream, "velocity_pid");
parameter_scalar_declare(&d->config.position_pid_stream, &d->config.stream, "position_pid");
parameter_scalar_declare(&d->config.index_stream, &d->config.stream, "index");
parameter_scalar_declare(&d->config.encoder_pos_stream, &d->config.stream, "encoder_pos");
parameter_scalar_declare(&d->config.motor_pos_stream, &d->config.stream, "motor_pos");
parameter_scalar_declare(&d->config.motor_torque_stream, &d->config.stream, "motor_torque");
d->stream.change_status = 0;
}
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 can_bridge_start(BaseChannel *ch)
{
static struct io_dev_s io_dev;
io_dev.channel = ch;
chBSemObjectInit(&io_dev.lock, false);
can_driver_start();
chThdCreateStatic(receiver_therad_wa, sizeof(receiver_therad_wa),
NORMALPRIO, receiver_therad, &io_dev);
chThdCreateStatic(sender_thread_wa, sizeof(sender_thread_wa),
NORMALPRIO, sender_thread, &io_dev);
}
/** Enable the USART peripherals.
* USART 6, 1 and 3 peripherals are configured
* (connected to the FTDI, UARTA and UARTB ports on the Piksi respectively).
*/
void usarts_enable(u32 ftdi_baud, u32 uarta_baud, u32 uartb_baud, bool do_preconfigure_hooks)
{
/* Ensure that the first time around, we do the preconfigure hooks */
if (!all_uarts_enabled && !do_preconfigure_hooks)
return;
usart_support_init();
usart_support_set_parameters(SD_FTDI, ftdi_baud);
usart_support_set_parameters(SD_UARTA, uarta_baud);
usart_support_set_parameters(SD_UARTB, uartb_baud);
chBSemObjectInit(&ftdi_state.claimed, FALSE);
ftdi_state.configured = true;
if (do_preconfigure_hooks) {
board_preinit_hook();
log_info("Piksi Starting...");
log_info("Firmware Version: " GIT_VERSION "");
log_info("Built: " __DATE__ " " __TIME__ "");
if (uarta_usart.configure_telemetry_radio_on_boot) {
radio_preconfigure_hook(UARTA, uarta_baud, "UARTA");
}
if (uartb_usart.configure_telemetry_radio_on_boot) {
radio_preconfigure_hook(UARTB, uartb_baud, "UARTB");
}
}
chBSemObjectInit(&uarta_state.claimed, FALSE);
uarta_state.configured = true;
chBSemObjectInit(&uartb_state.claimed, FALSE);
uartb_state.configured = true;
all_uarts_enabled = true;
}
static void usart_support_init_rx(struct usart_support_s *sd)
{
struct usart_rx_dma_state *s = &sd->rx;
s->rd = s->rd_wraps = s->wr_wraps = 0;
COMPILER_BARRIER();
/* Setup RX DMA */
dmaStreamSetMode(sd->rx.dma, sd->dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE |
STM32_DMA_CR_CIRC);
dmaStreamSetTransactionSize(sd->rx.dma, USART_RX_BUFFER_LEN);
dmaStreamSetPeripheral(sd->rx.dma, &sd->usart->DR);
dmaStreamSetMemory0(sd->rx.dma, sd->rx.buff);
chBSemObjectInit(&sd->rx.ready, TRUE);
dmaStreamEnable(sd->rx.dma);
}
void bus_power_init(void)
{
chBSemObjectInit(&adc_wait, true);
adcStart(&ADCD1, NULL);
}
/*------------------------------------------------------------------------*
* chibios_rt::BinarySemaphore *
*------------------------------------------------------------------------*/
BinarySemaphore::BinarySemaphore(bool taken) {
chBSemObjectInit(&bsem, taken);
}
/****************** Thread main loop ***********************************/
msg_t analogue_thread(void *args)
{
(void)args;
chRegSetThreadName("Analogue");
chBSemObjectInit(&bsAnalogueInst, true);
chBSemObjectInit(&bsAnalogueFX, true);
adcInit();
adcStart(&ADCD1, NULL);
adcStart(&ADCD2, NULL);
adcStartConversion(&ADCD1, &adc_con_group_1, (adcsample_t*)buffer1,
INST_BUF_DEPTH);
adcStartConversion(&ADCD2, &adc_con_group_2, (adcsample_t*)fx_samples,
FX_BUF_DEPTH);
dacInit();
dacStart(&DACD1, &dac_cfg);
dacStartConversion(&DACD1, &dac_conv_grp, (dacsample_t*)buffer3,
INST_BUF_DEPTH);
// Enable DAC output buffer:
DACD1.params->dac->CR |= DAC_CR_BOFF1;
/* Start the GPT timers. They reload at after reaching 1 such that
* TRGO frequency equals timer frequency. */
gptStart(&GPTD3, &gpt_inst_config);
GPTD3.tim->CR2 |= STM32_TIM_CR2_MMS(2);
gptStartContinuous(&GPTD3, 2);
GPTD3.tim->DIER &= ~STM32_TIM_DIER_UIE;
gptStart(&GPTD8, &gpt_fx_config);
GPTD8.tim->CR2 |= STM32_TIM_CR2_MMS(2);
gptStartContinuous(&GPTD8, 2);
GPTD8.tim->DIER &= ~STM32_TIM_DIER_UIE;
state = 1;
// States:
// 1 - ADC:buf1, DSP:buf2, DAC:buf3
// 2 - DSP:buf1, DAC:buf2, ADC:buf3
// 3 - DAC:buf1, ADC:buf2, DSP:buf3
/* Wait until the ADC callback boops the semaphore. */
volatile uint16_t *dsp_buf;
while(true) {
chSysLock();
chBSemWaitS(&bsAnalogueInst);
chSysUnlock();
state += 1;
switch(state)
{
case 1:
dmaSetOtherMemory(ADCD1.dmastp, buffer1);
dsp_buf = buffer2;
dmaSetOtherMemory(DACD1.params->dma, buffer3);
break;
case 2:
dmaSetOtherMemory(ADCD1.dmastp, buffer3);
dsp_buf = buffer1;
dmaSetOtherMemory(DACD1.params->dma, buffer2);
break;
case 3:
dmaSetOtherMemory(ADCD1.dmastp, buffer2);
dsp_buf = buffer3;
dmaSetOtherMemory(DACD1.params->dma, buffer1);
break;
default:
state = 1;
dmaSetOtherMemory(ADCD1.dmastp, buffer1);
dsp_buf = buffer2;
dmaSetOtherMemory(DACD1.params->dma, buffer3);
}
dsp_stuff(dsp_buf);
}
}
THD_FUNCTION(LogThread, arg)
{
(void) arg;
uint32_t FlashAddress;
chRegSetThreadName("Flash Logger");
int16_t i;
OpCode = NO_OPCODE;
FlashSate = FLASH_IDLE;
FlashAddress = 0;
chBSemObjectInit(&FlashSemaphore, TRUE); /* Semaphore initialization before use */
while (TRUE)
{
chBSemWait(&FlashSemaphore);
switch (OpCode)
{
case NO_OPCODE:
break;
case RECORDING_OPCODE:
TURN_LED_ON();
M25P16SetWriteEnable();
M25P16BulkErase();
FlashSate = FLASH_WAIT_FOR_PAGE;
FlashAddress = 0;
TURN_LED_OFF();
break;
case WRITEPAGE_OPCODE:
M25P16WritePage(FlashAddress, DataBuffer);
FlashAddress += PAGE_SIZE;
if (FlashAddress < LAST_FLASH_ADDRESS)
FlashSate = FLASH_WAIT_FOR_PAGE;
else
{
FlashSate = FLASH_FINISHED;
TURN_LED_OFF();
chThdSleepMilliseconds(1000);
TURN_LED_ON();
while (TRUE)
chThdSleep(TIME_INFINITE);
}
break;
case READING_OPCODE:
TURN_LED_ON();
FlashAddress = 0;
FlashSate = FLASH_FINISHED;
DataBuffer = (uint8_t *) LogData;
while (FlashAddress < LAST_FLASH_ADDRESS)
{
M25P16ReadPage(FlashAddress, DataBuffer);
for (i=0;i<PAGE_SIZE/16;i++)
{
if (SDU1.config->usbp->state == USB_ACTIVE)
{
chnWrite(&SDU1, &(DataBuffer[16*i]),16);
}
// chThdSleepMicroseconds(50);
}
FlashAddress += PAGE_SIZE;
}
TURN_LED_OFF();
while (TRUE)
chThdSleep(TIME_INFINITE);
break;
}
}
}
