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