void spiInitDevice(SPIDevice device) { spiDevice_t *spi = &(spiHardwareMap[device]); #ifdef SDCARD_SPI_INSTANCE if (spi->dev == SDCARD_SPI_INSTANCE) { spi->leadingEdge = true; } #endif #ifdef RX_SPI_INSTANCE if (spi->dev == RX_SPI_INSTANCE) { spi->leadingEdge = true; } #endif // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); if (spi->nss) { IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); } #endif #if defined(STM32F10X) IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); if (spi->nss) { IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); } #endif spiHardwareMap[device].hspi.Instance = spi->dev; // Init SPI hardware HAL_SPI_DeInit(&spiHardwareMap[device].hspi); spiHardwareMap[device].hspi.Init.Mode = SPI_MODE_MASTER; spiHardwareMap[device].hspi.Init.Direction = SPI_DIRECTION_2LINES; spiHardwareMap[device].hspi.Init.DataSize = SPI_DATASIZE_8BIT; spiHardwareMap[device].hspi.Init.NSS = SPI_NSS_SOFT; spiHardwareMap[device].hspi.Init.FirstBit = SPI_FIRSTBIT_MSB; spiHardwareMap[device].hspi.Init.CRCPolynomial = 7; spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; spiHardwareMap[device].hspi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; spiHardwareMap[device].hspi.Init.TIMode = SPI_TIMODE_DISABLED; if (spi->leadingEdge) { spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_LOW; spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_1EDGE; } else { spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_HIGH; spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE; } if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) { if (spi->nss) { IOHi(IOGetByTag(spi->nss)); } } }
void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock) { max7456HardwareReset(); #ifdef MAX7456_SPI_CS_PIN max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); #endif IOInit(max7456CsPin, OWNER_OSD_CS, 0); IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); IOHi(max7456CsPin); // Detect device type by writing and reading CA[8] bit at CMAL[6]. // Do this at half the speed for safety. spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK * 2); max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) { max7456DeviceType = MAX7456_DEVICE_TYPE_AT; } else { max7456DeviceType = MAX7456_DEVICE_TYPE_MAX; } #if defined(USE_OVERCLOCK) // Determine SPI clock divisor based on config and the device type. switch (max7456Config->clockConfig) { case MAX7456_CLOCK_CONFIG_HALF: max7456SpiClock = MAX7456_SPI_CLK * 2; break; case MAX7456_CLOCK_CONFIG_OC: max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK; break; case MAX7456_CLOCK_CONFIG_FULL: max7456SpiClock = MAX7456_SPI_CLK; break; } DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, cpuOverclock); DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DEVTYPE, max7456DeviceType); DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClock); #else UNUSED(max7456Config); UNUSED(cpuOverclock); #endif spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock); // force soft reset on Max7456 ENABLE_MAX7456; max7456Send(MAX7456ADD_VM0, MAX7456_RESET); DISABLE_MAX7456; // Setup values to write to registers videoSignalCfg = pVcdProfile->video_system; hosRegValue = 32 - pVcdProfile->h_offset; vosRegValue = 16 - pVcdProfile->v_offset; #ifdef MAX7456_DMA_CHANNEL_TX dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0); #endif // Real init will be made later when driver detect idle. }
void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = 29; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM3, &TIM_OCInitStructure); TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); TIM_CtrlPWMOutputs(TIM3, ENABLE); /* configure DMA */ /* DMA clock enable */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); /* DMA1 Channel6 Config */ DMA_DeInit(DMA1_Channel6); DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(DMA1_Channel6, &DMA_InitStructure); /* TIM3 CC1 DMA Request enable */ TIM_DMACmd(TIM3, TIM_DMA_CC1, ENABLE); DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); }
void i2cInit(I2CDevice device) { i2cDevice_t *i2c; i2c = &(i2cHardwareMap[device]); I2C_TypeDef *I2Cx; I2Cx = i2c->dev; IO_t scl = IOGetByTag(i2c->scl); IO_t sda = IOGetByTag(i2c->sda); RCC_ClockCmd(i2c->rcc, ENABLE); RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK); IOInit(scl, OWNER_I2C, RESOURCE_I2C_SCL, RESOURCE_INDEX(device)); IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_4); IOInit(sda, OWNER_I2C, RESOURCE_I2C_SDA, RESOURCE_INDEX(device)); IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_4); I2C_InitTypeDef i2cInit = { .I2C_Mode = I2C_Mode_I2C, .I2C_AnalogFilter = I2C_AnalogFilter_Enable, .I2C_DigitalFilter = 0x00, .I2C_OwnAddress1 = 0x00, .I2C_Ack = I2C_Ack_Enable, .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, .I2C_Timing = (i2c->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING) }; I2C_Init(I2Cx, &i2cInit); I2C_StretchClockCmd(I2Cx, ENABLE); I2C_Cmd(I2Cx, ENABLE); } uint16_t i2cGetErrorCounter(void) { return i2cErrorCount; } bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) { addr_ <<= 1; I2C_TypeDef *I2Cx; I2Cx = i2cHardwareMap[device].dev; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) { if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(); } } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(); } } /* Send Register address */ I2C_SendData(I2Cx, (uint8_t) reg); /* Wait until TCR flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) { if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(); } } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop); /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(); } } /* Write data to TXDR */ I2C_SendData(I2Cx, data); /* Wait until STOPF flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { if ((i2cTimeout--) == 0) { return i2cTimeoutUserCallback(); } } /* Clear STOPF flag */ I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); return true; }
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { uint32_t timerMhzCounter = 0; pwmWriteFuncPtr pwmWritePtr; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool isDigital = false; switch (motorConfig->motorPwmProtocol) { default: case PWM_TYPE_ONESHOT125: timerMhzCounter = ONESHOT125_TIMER_MHZ; pwmWritePtr = pwmWriteOneShot125; break; case PWM_TYPE_ONESHOT42: timerMhzCounter = ONESHOT42_TIMER_MHZ; pwmWritePtr = pwmWriteOneShot42; break; case PWM_TYPE_MULTISHOT: timerMhzCounter = MULTISHOT_TIMER_MHZ; pwmWritePtr = pwmWriteMultiShot; break; case PWM_TYPE_BRUSHED: timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; pwmWritePtr = pwmWriteBrushed; useUnsyncedPwm = true; idlePulse = 0; break; case PWM_TYPE_STANDARD: timerMhzCounter = PWM_TIMER_MHZ; pwmWritePtr = pwmWriteStandard; useUnsyncedPwm = true; idlePulse = 0; break; #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; break; #endif } if (!useUnsyncedPwm && !isDigital) { pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate; } for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { const ioTag_t tag = motorConfig->ioTags[motorIndex]; if (!tag) { break; } const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY); if (timerHardware == NULL) { /* flag failure and disable ability to arm */ break; } motors[motorIndex].io = IOGetByTag(tag); #ifdef USE_DSHOT if (isDigital) { pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol); motors[motorIndex].pwmWritePtr = pwmWriteDigital; motors[motorIndex].enabled = true; continue; } #endif IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP); motors[motorIndex].pwmWritePtr = pwmWritePtr; if (useUnsyncedPwm) { const uint32_t hz = timerMhzCounter * 1000000; pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); } else { pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); } motors[motorIndex].enabled = true; } }
void adcInit(const adcConfig_t *config) { uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcOperatingConfig, 0, sizeof(adcOperatingConfig)); if (config->vbat.enabled) { adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag; } if (config->rssi.enabled) { adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; //RSSI_ADC_CHANNEL; } if (config->external1.enabled) { adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; //EXTERNAL1_ADC_CHANNEL; } if (config->current.enabled) { adcOperatingConfig[ADC_CURRENT].tag = config->current.ioTag; //CURRENT_METER_ADC_CHANNEL; } ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); if (device == ADCINVALID) return; adcDevice_t adc = adcHardware[device]; bool adcActive = false; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcVerifyPin(adcOperatingConfig[i].tag, device)) { continue; } adcActive = true; IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0); IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL)); adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag); adcOperatingConfig[i].dmaIndex = configuredAdcChannels++; adcOperatingConfig[i].sampleTime = ADC_SAMPLETIME_480CYCLES; adcOperatingConfig[i].enabled = true; } if (!adcActive) { return; } RCC_ClockCmd(adc.rccADC, ENABLE); dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0); adc.ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV8; adc.ADCHandle.Init.ContinuousConvMode = ENABLE; adc.ADCHandle.Init.Resolution = ADC_RESOLUTION_12B; adc.ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1; adc.ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; adc.ADCHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT; adc.ADCHandle.Init.NbrOfConversion = configuredAdcChannels; adc.ADCHandle.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group adc.ADCHandle.Init.DiscontinuousConvMode = DISABLE; adc.ADCHandle.Init.NbrOfDiscConversion = 0; adc.ADCHandle.Init.DMAContinuousRequests = ENABLE; adc.ADCHandle.Init.EOCSelection = DISABLE; adc.ADCHandle.Instance = adc.ADCx; /*##-1- Configure the ADC peripheral #######################################*/ if (HAL_ADC_Init(&adc.ADCHandle) != HAL_OK) { /* Initialization Error */ } adc.DmaHandle.Init.Channel = adc.channel; adc.DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; adc.DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; adc.DmaHandle.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE; adc.DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; adc.DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; adc.DmaHandle.Init.Mode = DMA_CIRCULAR; adc.DmaHandle.Init.Priority = DMA_PRIORITY_HIGH; adc.DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; adc.DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; adc.DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE; adc.DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; adc.DmaHandle.Instance = adc.DMAy_Streamx; /*##-2- Initialize the DMA stream ##########################################*/ if (HAL_DMA_Init(&adc.DmaHandle) != HAL_OK) { /* Initialization Error */ } __HAL_LINKDMA(&adc.ADCHandle, DMA_Handle, adc.DmaHandle); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcOperatingConfig[i].enabled) { continue; } ADC_ChannelConfTypeDef sConfig; sConfig.Channel = adcOperatingConfig[i].adcChannel; sConfig.Rank = rank++; sConfig.SamplingTime = adcOperatingConfig[i].sampleTime; sConfig.Offset = 0; /*##-3- Configure ADC regular channel ######################################*/ if (HAL_ADC_ConfigChannel(&adc.ADCHandle, &sConfig) != HAL_OK) { /* Channel Configuration Error */ } } //HAL_CLEANINVALIDATECACHE((uint32_t*)&adcValues, configuredAdcChannels); /*##-4- Start the conversion process #######################################*/ if (HAL_ADC_Start_DMA(&adc.ADCHandle, (uint32_t*)&adcValues, configuredAdcChannels) != HAL_OK) { /* Start Conversation Error */ } }
void i2cInit(I2CDevice device) { if (device == I2CINVALID) { return; } i2cDevice_t *pDev = &i2cDevice[device]; const i2cHardware_t *hardware = pDev->hardware; if (!hardware) { return; } IO_t scl = pDev->scl; IO_t sda = pDev->sda; IOInit(scl, OWNER_I2C_SCL, RESOURCE_INDEX(device)); IOInit(sda, OWNER_I2C_SDA, RESOURCE_INDEX(device)); // Enable RCC RCC_ClockCmd(hardware->rcc, ENABLE); i2cUnstick(scl, sda); // Init pins #ifdef STM32F7 IOConfigGPIOAF(scl, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C); IOConfigGPIOAF(sda, pDev->pullUp ? IOCFG_I2C_PU : IOCFG_I2C, GPIO_AF4_I2C); #else IOConfigGPIO(scl, IOCFG_AF_OD); IOConfigGPIO(sda, IOCFG_AF_OD); #endif // Init I2C peripheral I2C_HandleTypeDef *pHandle = &pDev->handle; memset(pHandle, 0, sizeof(*pHandle)); pHandle->Instance = pDev->hardware->reg; /// TODO: HAL check if I2C timing is correct if (pDev->overClock) { // 800khz Maximum speed tested on various boards without issues pHandle->Init.Timing = 0x00500D1D; } else { pHandle->Init.Timing = 0x00500C6F; } pHandle->Init.OwnAddress1 = 0x0; pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; pHandle->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; pHandle->Init.OwnAddress2 = 0x0; pHandle->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; pHandle->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; HAL_I2C_Init(pHandle); // Enable the Analog I2C Filter HAL_I2CEx_ConfigAnalogFilter(pHandle, I2C_ANALOGFILTER_ENABLE); // Setup interrupt handlers HAL_NVIC_SetPriority(hardware->er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER)); HAL_NVIC_EnableIRQ(hardware->er_irq); HAL_NVIC_SetPriority(hardware->ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV)); HAL_NVIC_EnableIRQ(hardware->ev_irq); }
void adcInit(drv_adc_config_t *init) { #if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN) UNUSED(init); #endif uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); #ifdef VBAT_ADC_PIN if (init->enableVBat) { adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); } #endif ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); if (device == ADCINVALID) return; const adcDevice_t adc = adcHardware[device]; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) continue; IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_BATTERY+i, 0); IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AIN, 0)); adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); adcConfig[i].dmaIndex = configuredAdcChannels++; adcConfig[i].sampleTime = ADC_SampleTime_239Cycles5; adcConfig[i].enabled = true; } RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI) RCC_ClockCmd(adc.rccADC, ENABLE); RCC_ClockCmd(adc.rccDMA, ENABLE); DMA_DeInit(adc.DMAy_Channelx); DMA_InitTypeDef DMA_InitStructure; DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_BufferSize = configuredAdcChannels; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = configuredAdcChannels > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(adc.DMAy_Channelx, &DMA_InitStructure); DMA_Cmd(adc.DMAy_Channelx, ENABLE); ADC_InitTypeDef ADC_InitStructure; ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfChannel = configuredAdcChannels; ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } ADC_RegularChannelConfig(adc.ADCx, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } ADC_DMACmd(adc.ADCx, ENABLE); ADC_Cmd(adc.ADCx, ENABLE); ADC_ResetCalibration(adc.ADCx); while (ADC_GetResetCalibrationStatus(adc.ADCx)); ADC_StartCalibration(adc.ADCx); while (ADC_GetCalibrationStatus(adc.ADCx)); ADC_SoftwareStartConvCmd(adc.ADCx, ENABLE); }
void max7456_init(uint8_t video_system) { uint8_t max_screen_rows; uint8_t srdata = 0; uint16_t x; #ifdef MAX7456_SPI_CS_PIN max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); #endif IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0); IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); //Minimum spi clock period for max7456 is 100ns (10Mhz) spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); delay(1000); // force soft reset on Max7456 ENABLE_MAX7456; max7456_send(VM0_REG, MAX7456_RESET); delay(100); srdata = max7456_send(0xA0, 0xFF); if ((0x01 & srdata) == 0x01) { //PAL video_signal_type = VIDEO_MODE_PAL; } else if ((0x02 & srdata) == 0x02) { //NTSC video_signal_type = VIDEO_MODE_NTSC; } // Override detected type: 0-AUTO, 1-PAL, 2-NTSC switch(video_system) { case PAL: video_signal_type = VIDEO_MODE_PAL; break; case NTSC: video_signal_type = VIDEO_MODE_NTSC; break; } if (video_signal_type) { //PAL max_screen_size = VIDEO_BUFFER_CHARS_PAL; max_screen_rows = VIDEO_LINES_PAL; } else { // NTSC max_screen_size = VIDEO_BUFFER_CHARS_NTSC; max_screen_rows = VIDEO_LINES_NTSC; } // set all rows to same charactor black/white level for(x = 0; x < max_screen_rows; x++) { max7456_send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); } // make sure the Max7456 is enabled max7456_send(VM0_REG, OSD_ENABLE | video_signal_type); DISABLE_MAX7456; delay(100); for (x = 0; x < max_screen_size; x++) SCREEN_BUFFER[x] = MAX7456_CHAR(' '); #ifdef MAX7456_DMA_CHANNEL_TX max7456_screen[0] = (uint16_t)(MAX7456ADD_DMAH | (0 << 8)); max7456_screen[1] = (uint16_t)(MAX7456ADD_DMAL | (0 << 8)); max7456_screen[2] = (uint16_t)(MAX7456ADD_DMM | (1 << 8)); max7456_screen[max_screen_size + 3] = (uint16_t)(MAX7456ADD_DMDI | (0xFF << 8)); max7456_screen[max_screen_size + 4] = (uint16_t)(MAX7456ADD_DMM | (0 << 8)); dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0); #endif }
void adcInit(adcConfig_t *config) { ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcOperatingConfig, 0, sizeof(adcOperatingConfig)); if (config->vbat.enabled) { adcOperatingConfig[ADC_BATTERY].tag = config->vbat.ioTag; } if (config->rssi.enabled) { adcOperatingConfig[ADC_RSSI].tag = config->rssi.ioTag; //RSSI_ADC_CHANNEL; } if (config->external1.enabled) { adcOperatingConfig[ADC_EXTERNAL1].tag = config->external1.ioTag; //EXTERNAL1_ADC_CHANNEL; } if (config->currentMeter.enabled) { adcOperatingConfig[ADC_CURRENT].tag = config->currentMeter.ioTag; //CURRENT_METER_ADC_CHANNEL; } ADCDevice device = adcDeviceByInstance(ADC_INSTANCE); if (device == ADCINVALID) return; adcDevice_t adc = adcHardware[device]; bool adcActive = false; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcOperatingConfig[i].tag) continue; adcActive = true; IOInit(IOGetByTag(adcOperatingConfig[i].tag), OWNER_ADC_BATT + i, 0); IOConfigGPIO(IOGetByTag(adcOperatingConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag); adcOperatingConfig[i].dmaIndex = configuredAdcChannels++; adcOperatingConfig[i].sampleTime = ADC_SampleTime_480Cycles; adcOperatingConfig[i].enabled = true; } if (!adcActive) { return; } RCC_ClockCmd(adc.rccADC, ENABLE); dmaInit(dmaGetIdentifier(adc.DMAy_Streamx), OWNER_ADC, 0); DMA_DeInit(adc.DMAy_Streamx); DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&adc.ADCx->DR; DMA_InitStructure.DMA_Channel = adc.channel; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_BufferSize = configuredAdcChannels; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = configuredAdcChannels > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_Init(adc.DMAy_Streamx, &DMA_InitStructure); DMA_Cmd(adc.DMAy_Streamx, ENABLE); ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_CommonStructInit(&ADC_CommonInitStructure); ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; ADC_CommonInit(&ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcOperatingConfig[i].enabled) { continue; } ADC_RegularChannelConfig(adc.ADCx, adcOperatingConfig[i].adcChannel, rank++, adcOperatingConfig[i].sampleTime); } ADC_DMARequestAfterLastTransferCmd(adc.ADCx, ENABLE); ADC_DMACmd(adc.ADCx, ENABLE); ADC_Cmd(adc.ADCx, ENABLE); ADC_SoftwareStartConv(adc.ADCx); }
void adcInit(drv_adc_config_t *init) { uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); #ifdef VBAT_ADC_PIN if (init->enableVBat) { adcConfig[ADC_BATTERY].tag = IO_TAG(VBAT_ADC_PIN); //VBAT_ADC_CHANNEL; } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { adcConfig[ADC_RSSI].tag = IO_TAG(RSSI_ADC_PIN); //RSSI_ADC_CHANNEL; } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { adcConfig[ADC_EXTERNAL1].tag = IO_TAG(EXTERNAL1_ADC_PIN); //EXTERNAL1_ADC_CHANNEL; } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { adcConfig[ADC_CURRENT].tag = IO_TAG(CURRENT_METER_ADC_PIN); //CURRENT_METER_ADC_CHANNEL; } #endif __HAL_RCC_DMA2_CLK_ENABLE(); __HAL_RCC_ADC1_CLK_ENABLE(); AdcHandle_1.Instance = ADC1; AdcHandle_1.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV8; AdcHandle_1.Init.Resolution = ADC_RESOLUTION_12B; AdcHandle_1.Init.ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; AdcHandle_1.Init.ContinuousConvMode = ENABLE; AdcHandle_1.Init.DiscontinuousConvMode = DISABLE; AdcHandle_1.Init.NbrOfDiscConversion = 0; AdcHandle_1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; /* Conversion start trigged at each external event */ AdcHandle_1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1; AdcHandle_1.Init.DataAlign = ADC_DATAALIGN_RIGHT; AdcHandle_1.Init.NbrOfConversion = configuredAdcChannels; AdcHandle_1.Init.DMAContinuousRequests = ENABLE; AdcHandle_1.Init.EOCSelection = DISABLE; HAL_ADC_Init(&AdcHandle_1); hdma_adc_1.Instance = MCU_ADC1_DMA_STREAM; hdma_adc_1.Init.Channel = MCU_ADC1_DMA_CHANNEL; hdma_adc_1.Init.Direction = DMA_PERIPH_TO_MEMORY; hdma_adc_1.Init.PeriphInc = DMA_PINC_DISABLE; hdma_adc_1.Init.MemInc = configuredAdcChannels > 1 ? DMA_MINC_ENABLE : DMA_MINC_DISABLE;; hdma_adc_1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; hdma_adc_1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; hdma_adc_1.Init.Mode = DMA_CIRCULAR; hdma_adc_1.Init.Priority = DMA_PRIORITY_HIGH; hdma_adc_1.Init.FIFOMode = DMA_FIFOMODE_DISABLE; hdma_adc_1.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_HALFFULL; hdma_adc_1.Init.MemBurst = DMA_MBURST_SINGLE; hdma_adc_1.Init.PeriphBurst = DMA_PBURST_SINGLE; HAL_DMA_Init(&hdma_adc_1); /* Associate the initialized DMA handle to the ADC handle */ __HAL_LINKDMA(&AdcHandle_1, DMA_Handle, hdma_adc_1); /* NVIC configuration for DMA transfer complete interrupt */ // HAL_NVIC_SetPriority(MCU_ADC1_DMA_STREAM_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_ADC1_DMA), NVIC_PRIORITY_SUB(NVIC_PRIO_ADC1_DMA)); // HAL_NVIC_EnableIRQ(MCU_ADC1_DMA_STREAM_IRQn); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_BATTERY + i, 0); IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); ADC_ChannelConfTypeDef sConfig; sConfig.Channel = adcChannelByTag(adcConfig[i].tag); sConfig.Rank = rank++; sConfig.SamplingTime = ADC_SAMPLETIME_480CYCLES; sConfig.Offset = 0; HAL_ADC_ConfigChannel(&AdcHandle_1, &sConfig); } HAL_ADC_Start_DMA(&AdcHandle_1, (uint32_t*)&adcValues, configuredAdcChannels); }
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; TIM_TypeDef *timer = timerHardware->tim; const IO_t motorIO = IOGetByTag(timerHardware->tag); const uint8_t timerIndex = getTimerIndex(timer); IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction); __DMA1_CLK_ENABLE(); RCC_ClockCmd(timerRCC(timer), ENABLE); motor->TimHandle.Instance = timerHardware->tim; motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1; motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; motor->TimHandle.Init.RepetitionCounter = 0; motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; motor->TimHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_PWM_Init(&motor->TimHandle) != HAL_OK) { /* Initialization Error */ return; } motor->timerDmaSource = timerDmaSource(timerHardware->channel); dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource; /* Set the parameters to be configured */ motor->hdma_tim.Init.Channel = timerHardware->dmaChannel; motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE; motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; motor->hdma_tim.Init.Mode = DMA_NORMAL; motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; motor->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; motor->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; motor->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; /* Set hdma_tim instance */ if (timerHardware->dmaRef == NULL) { /* Initialization Error */ return; } motor->hdma_tim.Instance = timerHardware->dmaRef; /* Link hdma_tim to hdma[x] (channelx) */ __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); /* Initialize TIMx DMA handle */ if (HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaSource]) != HAL_OK) { /* Initialization Error */ return; } TIM_OC_InitTypeDef TIM_OCInitStructure; /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; if (output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; } else { TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET; TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH; } TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; if (HAL_TIM_PWM_ConfigChannel(&motor->TimHandle, &TIM_OCInitStructure, motor->timerHardware->channel) != HAL_OK) { /* Configuration Error */ return; } }
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; TIM_TypeDef *timer = timerHardware->tim; const IO_t motorIO = IOGetByTag(timerHardware->tag); const uint8_t timerIndex = getTimerIndex(timer); const bool configureTimer = (timerIndex == dmaMotorTimerCount-1); IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); RCC_ClockCmd(timerRCC(timer), ENABLE); TIM_Cmd(timer, DISABLE); uint32_t hz; switch (pwmProtocolType) { case(PWM_TYPE_DSHOT600): hz = MOTOR_DSHOT600_MHZ * 1000000; break; case(PWM_TYPE_DSHOT300): hz = MOTOR_DSHOT300_MHZ * 1000000; break; default: case(PWM_TYPE_DSHOT150): hz = MOTOR_DSHOT150_MHZ * 1000000; } TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((SystemCoreClock / timerClockDivisor(timer) / hz) - 1); TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); motor->timerDmaSource = timerDmaSource(timerHardware->channel); dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource; TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); if (configureTimer) { TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); TIM_Cmd(timer, ENABLE); } DMA_Channel_TypeDef *channel = timerHardware->dmaChannel; if (channel == NULL) { /* trying to use a non valid channel */ return; } dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); DMA_Cmd(channel, DISABLE); DMA_DeInit(channel); DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(channel, &DMA_InitStructure); DMA_ITConfig(channel, DMA_IT_TC, ENABLE); }
void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint8_t i; uint8_t adcChannelCount = 0; memset(&adcConfig, 0, sizeof(adcConfig)); #ifdef VBAT_ADC_PIN if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_BATTERY].enabled = true; adcChannelCount++; } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_RSSI].enabled = true; adcChannelCount++; } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_CURRENT].enabled = true; adcChannelCount++; } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_EXTERNAL1].enabled = true; adcChannelCount++; } #endif RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL | RCC_AHBPeriph_ADC12, ENABLE); DMA_DeInit(ADC_DMA_CHANNEL); DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_BufferSize = adcChannelCount; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = adcChannelCount > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure); DMA_Cmd(ADC_DMA_CHANNEL, ENABLE); // calibrate ADC_VoltageRegulatorCmd(ADC_INSTANCE, ENABLE); delay(10); ADC_SelectCalibrationMode(ADC_INSTANCE, ADC_CalibrationMode_Single); ADC_StartCalibration(ADC_INSTANCE); while(ADC_GetCalibrationStatus(ADC_INSTANCE) != RESET); ADC_VoltageRegulatorCmd(ADC_INSTANCE, DISABLE); ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_CommonStructInit(&ADC_CommonInitStructure); ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; ADC_CommonInitStructure.ADC_Clock = ADC_Clock_SynClkModeDiv4; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular; ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0; ADC_CommonInit(ADC_INSTANCE, &ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_ContinuousConvMode = ADC_ContinuousConvMode_Enable; ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ExternalTrigConvEvent = ADC_ExternalTrigConvEvent_0; ADC_InitStructure.ADC_ExternalTrigEventEdge = ADC_ExternalTrigEventEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_OverrunMode = ADC_OverrunMode_Disable; ADC_InitStructure.ADC_AutoInjMode = ADC_AutoInjec_Disable; ADC_InitStructure.ADC_NbrOfRegChannel = adcChannelCount; ADC_Init(ADC_INSTANCE, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } ADC_Cmd(ADC_INSTANCE, ENABLE); while(!ADC_GetFlagStatus(ADC_INSTANCE, ADC_FLAG_RDY)); ADC_DMAConfig(ADC_INSTANCE, ADC_DMAMode_Circular); ADC_DMACmd(ADC_INSTANCE, ENABLE); ADC_StartConversion(ADC_INSTANCE); }
/** * Function to set the SPI module. * \param obj - SPIContext pointer * \param options - SPI options * \param pin - SlaveSelect pin, use SPI_OPT_NO_SS if not necessary * \param speed - desired connection speed. System will automatically calculate the nearest possible speed * \return - 0 the operation is successful * \return - 1 the operation is failed. Check internal error for more details */ BOOL SPIConfig(SPIContext * obj, int options, int pin, long speed) { double tout=0; obj->delay=100; obj->SPICON1=0; obj->SPICON2=0; obj->SPISTAT=0; obj->primary=0; obj->secondary=0; obj->ss_pin=0; obj->SPIIE=0; obj->SPFIE=0; obj->mode16 = FALSE; int index=0; #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Initiating configuration\n"); #endif if(speed > _SPI_MAX_SPD) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Speed too fast during config, quitting.\n"); #endif _intSPIerr = SpdTooFast; return 1; } unsigned int divider = (unsigned int)ceil(16000000.0/(double)speed); #ifdef SPI_DBG_CFG sprintf(spi_dbg_msg,"[SPI_DBG] Calculated divider: %d\n",divider); UARTWrite(1,spi_dbg_msg); #endif index = _spi_find_divider_index(divider); obj->primary = _spi_divider_primary[index]; obj->secondary = _spi_divider_secondary[index]; obj->ss_pin = pin; #ifdef SPI_DBG_CFG sprintf(spi_dbg_msg,"[SPI_DBG] primary %d\n[SPI_DBG] secondary %d\n",_spi_divider_primary_value[obj->primary],_spi_divider_secondary_value[obj->secondary]); uartwrite(1,spi_dbg_msg); #endif tout = ((float)_spi_divider_primary_value[obj->primary] * (float)_spi_divider_secondary_value[obj->secondary])/16; // time (in us) per bit shifted out if (options & SPI_OPT_MODE16) { obj->mode16 = TRUE; obj->SPICON1 |= SPI_MSK_MODE16; obj->delay = ceil(32*tout/10); } else obj->delay = ceil(16*tout/10); if (obj->delay < 1) obj->delay = 1; #ifdef SPI_DBG_CFG sprintf(spi_dbg_msg,"[SPI_DBG] Requested speed %.2f KHz\n[SPI_DBG] Nearest speed %.2f KHz\n",(double)speed/1000,1000/tout); uartwrite(1,spi_dbg_msg); #endif if((options & SPI_OPT_MODE_0) || (!(options & (SPI_OPT_MODE_0 | SPI_OPT_MODE_1 | SPI_OPT_MODE_2 | SPI_OPT_MODE_3)))) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Selecting MODE 0\n"); #endif obj->SPICON1 |= SPI_MSK_CPHA; //288 // bit5=1 master mode + bit8=1 data changes on clock trailing edge obj->SPICON1 &= ~SPI_MSK_CPOL; } if(options & SPI_OPT_MODE_1) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Selecting MODE 1\n"); #endif obj->SPICON1 &= ~SPI_MSK_CPHA; obj->SPICON1 &= ~SPI_MSK_CPOL; //32// bit5=1 master mode } if(options & SPI_OPT_MODE_2) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Selecting MODE 2\n"); #endif obj->SPICON1 |= SPI_MSK_CPHA | SPI_MSK_CPOL; //352 // bit5=1 master mode + bit8=1 data changes on clock trailing edge + bit6=1 clock polarity reversed } if(options & SPI_OPT_MODE_3) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] Selecting MODE 3\n"); #endif obj->SPICON1 |= SPI_MSK_CPOL; obj->SPICON1 &= ~SPI_MSK_CPHA; //96 // bit5=1 master mode + bit6=1 clock polarity } obj->SPICON1 |= obj->primary&0b00000011; obj->SPICON1 |= (obj->secondary&0b00011100)<<2; if(options & SPI_OPT_MASTER) obj->SPICON1 |= SPI_MSK_MASTER; if(options & SPI_OPT_SLAVE) obj->SPICON1 &= ~SPI_MSK_MASTER; //0b1111111111011111; if(options & SPI_OPT_SLAVE_SELECT) obj->SPICON1 |= SPI_MSK_SSEN; if ((options & (SPI_OPT_SLAVE | SPI_OPT_MASTER)) == 0) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] No mode (Master/Slave) selected, quitting\n"); #endif _intSPIerr=NoModeSpecified; obj->SPICON1=0; obj->SPICON2=0; obj->SPISTAT=0; obj->primary=0; obj->secondary=0; obj->ss_pin=0; return 1; } /*#if(GroveNest == Board) obj->SPICON1 |= 0b1000000; #endif*/ obj->SPISTAT = 0x00; obj->SPICON2 = 0x00; obj->SPIIE = 0; obj->SPFIE = 0; obj->IP = 1; obj->FIP = 1; #ifndef SPI2_FLASH if(pin != SPI_OPT_NO_SS) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] SS pin selected, configuring\n"); #endif obj->ss_pin = pin; IOInit(pin,out); vTaskDelay(1); IOPut(pin,on); #ifdef SPI_DBG_CFG sprintf(spi_dbg_msg,"[SPI_DBG] SS us of delay %u\n",obj->delay*10); UARTWrite(1,spi_dbg_msg); #endif } else { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] SS pin cleared, not using\n"); #endif obj->ss_pin=SPI_OPT_NO_SS; } #else TRISDbits.TRISD6=0; #endif if(options & SPI_OPT_DI_SAMPLE_END) { #ifdef SPI_DBG_CFG uartwrite(1,"[SPI_DBG] DI sample set to end of DO clock time\n"); #endif obj->SPICON1 |= 0b1000000000; } _intSPIerr=SPI_NO_ERR; return 0; }
// USART1 - Telemetry (RX/TX by DMA) uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE]; static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE]; s = &uartPort1; s->port.vTable = uartVTable; s->port.baudRate = baudRate; s->port.rxBuffer = rx1Buffer; s->port.txBuffer = tx1Buffer; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE; s->USARTx = USART1; #ifdef USE_UART1_RX_DMA dmaInit(DMA1_CH5_HANDLER, OWNER_SERIAL_RX, 1); s->rxDMAChannel = DMA1_Channel5; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; #endif s->txDMAChannel = DMA1_Channel4; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; RCC_ClockCmd(RCC_APB2(USART1), ENABLE); // UART1_TX PA9 // UART1_RX PA10 if (options & SERIAL_BIDIR) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL_TX, 1); IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_OD); } else { if (mode & MODE_TX) { IOInit(IOGetByTag(IO_TAG(PA9)), OWNER_SERIAL_TX, 1); IOConfigGPIO(IOGetByTag(IO_TAG(PA9)), IOCFG_AF_PP); } if (mode & MODE_RX) { IOInit(IOGetByTag(IO_TAG(PA10)), OWNER_SERIAL_RX, 1); IOConfigGPIO(IOGetByTag(IO_TAG(PA10)), IOCFG_IPU); } } // DMA TX Interrupt dmaInit(DMA1_CH4_HANDLER, OWNER_SERIAL_TX, 1); dmaSetHandler(DMA1_CH4_HANDLER, uart_tx_dma_IRQHandler, NVIC_PRIO_SERIALUART1_TXDMA, (uint32_t)&uartPort1); #ifndef USE_UART1_RX_DMA // RX/TX Interrupt NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); #endif return s; }
void hmc5883lInit(void) { int16_t magADC[3]; int i; int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator #ifdef USE_MAG_DATA_READY_SIGNAL if (hmc5883Config && hmc5883Config->io) { intIO = IOGetByTag(hmc5883Config->io); IOInit(intIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI); IOConfigGPIO(intIO, IOCFG_IN_FLOATING); } #endif delay(50); i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) delay(100); hmc5883lRead(magADC); for (i = 0; i < 10; i++) { // Collect 10 samples i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. // Since the measurements are noisy, they should be averaged rather than taking the max. xyz_total[X] += magADC[X]; xyz_total[Y] += magADC[Y]; xyz_total[Z] += magADC[Z]; // Detect saturation. if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } LED1_TOGGLE; } // Apply the negative bias. (Same gain) i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. for (i = 0; i < 10; i++) { i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. // Since the measurements are noisy, they should be averaged. xyz_total[X] -= magADC[X]; xyz_total[Y] -= magADC[Y]; xyz_total[Z] -= magADC[Z]; // Detect saturation. if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } LED1_TOGGLE; } magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); // leave test mode i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode delay(100); if (!bret) { // Something went wrong so get a best guess magGain[X] = 1.0f; magGain[Y] = 1.0f; magGain[Z] = 1.0f; } #ifdef USE_MAG_DATA_READY_SIGNAL do { if (!(hmc5883Config && intIO)) break; # ifdef ENSURE_MAG_DATA_READY_IS_HIGH if (!IORead(intIO)) break; # endif EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler); EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising); EXTIEnable(intIO, true); } while (0); #endif }
void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); #if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN) UNUSED(init); #endif #ifdef VBAT_ADC_PIN if (init->enableVBat) { IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_480Cycles; } #endif #ifdef EXTERNAL1_ADC_PIN if (init->enableExternal1) { IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++; adcConfig[ADC_EXTERNAL1].enabled = true; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_480Cycles; } #endif #ifdef RSSI_ADC_PIN if (init->enableRSSI) { IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++; adcConfig[ADC_RSSI].enabled = true; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_480Cycles; } #endif #ifdef CURRENT_METER_ADC_PIN if (init->enableCurrentMeter) { IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC); IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)); adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++; adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_480Cycles; } #endif //RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); DMA_DeInit(DMA2_Stream4); DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; DMA_InitStructure.DMA_Channel = DMA_Channel_0; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_BufferSize = configuredAdcChannels; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = configuredAdcChannels > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_Init(DMA2_Stream4, &DMA_InitStructure); DMA_Cmd(DMA2_Stream4, ENABLE); // calibrate /* ADC_VoltageRegulatorCmd(ADC1, ENABLE); delay(10); ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single); ADC_StartCalibration(ADC1); while(ADC_GetCalibrationStatus(ADC1) != RESET); ADC_VoltageRegulatorCmd(ADC1, DISABLE); */ ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_CommonStructInit(&ADC_CommonInitStructure); ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; ADC_CommonInit(&ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels; ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group ADC_Init(ADC1, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); ADC_DMACmd(ADC1, ENABLE); ADC_Cmd(ADC1, ENABLE); ADC_SoftwareStartConv(ADC1); }
static void serialOutputPortConfig(ioTag_t pin, uint8_t portIndex) { IOInit(IOGetByTag(pin), OWNER_SOFTSERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(portIndex)); IOConfigGPIO(IOGetByTag(pin), IOCFG_OUT_PP); }
void spiInitDevice(SPIDevice device) { SPI_InitTypeDef spiInit; spiDevice_t *spi = &(spiHardwareMap[device]); #ifdef SDCARD_SPI_INSTANCE if (spi->dev == SDCARD_SPI_INSTANCE) spi->sdcard = true; #endif // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); IOInit(IOGetByTag(spi->sck), OWNER_SPI, RESOURCE_SPI_SCK, device + 1); IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1); IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); #if defined(STM32F3) || defined(STM32F4) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); if (spi->nss) IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); #endif #if defined(STM32F10X) IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); if (spi->nss) IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); #endif // Init SPI hardware SPI_I2S_DeInit(spi->dev); spiInit.SPI_Mode = SPI_Mode_Master; spiInit.SPI_Direction = SPI_Direction_2Lines_FullDuplex; spiInit.SPI_DataSize = SPI_DataSize_8b; spiInit.SPI_NSS = SPI_NSS_Soft; spiInit.SPI_FirstBit = SPI_FirstBit_MSB; spiInit.SPI_CRCPolynomial = 7; spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; if (spi->sdcard) { spiInit.SPI_CPOL = SPI_CPOL_Low; spiInit.SPI_CPHA = SPI_CPHA_1Edge; } else { spiInit.SPI_CPOL = SPI_CPOL_High; spiInit.SPI_CPHA = SPI_CPHA_2Edge; } #ifdef STM32F303xC // Configure for 8-bit reads. SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF); #endif SPI_Init(spi->dev, &spiInit); SPI_Cmd(spi->dev, ENABLE); if (spi->nss) IOHi(IOGetByTag(spi->nss)); }
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins) { bool detected = false; #ifdef STM32F10X // enable AFIO for EXTI support RCC_ClockCmd(RCC_APB2(AFIO), ENABLE); #endif #if defined(STM32F3) || defined(STM32F4) RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this? #endif triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag); echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); if (IOGetOwner(triggerIO) != OWNER_FREE) { return false; } if (IOGetOwner(echoIO) != OWNER_FREE) { return false; } // trigger pin IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0); IOConfigGPIO(triggerIO, IOCFG_OUT_PP); IOLo(triggerIO); delay(100); // echo pin IOInit(echoIO, OWNER_SONAR_ECHO, 0); IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); // HC-SR04 echo line should be low by default and should return a response pulse when triggered if (IORead(echoIO) == false) { for (int i = 0; i < 5 && !detected; i++) { timeMs_t requestTime = millis(); hcsr04_start_reading(); while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) { if (IORead(echoIO) == true) { detected = true; break; } } } } if (detected) { // Hardware detected - configure the driver #ifdef USE_EXTI EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! EXTIEnable(echoIO, true); #endif dev->delayMs = 100; dev->maxRangeCm = HCSR04_MAX_RANGE_CM; dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; dev->init = &hcsr04_init; dev->update = &hcsr04_update; dev->read = &hcsr04_get_distance; return true; } else { // Not detected - free resources IORelease(triggerIO); IORelease(echoIO); return false; } }