void systemInit(bool overclock) { #ifdef STM32F303xC // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X_MD // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(overclock); #endif // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); #ifdef STM32F10X_MD // Turn on clocks for stuff we use RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); #endif RCC_ClearFlag(); enableGPIOPowerUsageAndNoiseReductions(); #ifdef STM32F10X_MD // Turn off JTAG port 'cause we're using the GPIO for leds #define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24) AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW; #endif ledInit(); beeperInit(); // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); #ifdef CC3D spiInit(SPI1); spiInit(SPI2); #endif #ifndef CC3D // Configure the rest of the stuff i2cInit(I2C2); #endif // sleep for 100ms delay(100); }
void systemInit(void) { checkForBootLoaderRequest(); SetSysClock(); // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // cache RCC->CSR value to use it in isMPUSoftreset() and others cachedRccCsrValue = RCC->CSR; /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ extern void *isr_vector_table_base; NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE); RCC_ClearFlag(); enableGPIOPowerUsageAndNoiseReductions(); // Init cycle counter cycleCounterInit(); memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); }
/* The configPOST_STOP_PROCESSING() macro is called when the MCU leaves its STOP low power mode. The macro is set in FreeRTOSConfig.h to call vMainPostStopProcessing(). */ void vMainPostStopProcessing( void ) { extern void SetSysClock( void ); /* The STOP low power mode has been exited. Reconfigure the system clocks ready for normally running again. */ SetSysClock(); }
void deepsleep(void) { // Request to enter STOP mode with regulator in low power mode HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI); // After wake-up from STOP reconfigure the PLL SetSysClock(); }
void systemClockSetup(uint8_t cpuUnderclock) { // Configure the RCC. Note that this should be called only once per boot SetSysClock(cpuUnderclock); // Re-initialize system timekeeping - CPU clock changed systemTimekeepingSetup(); }
void SystemInit (void) { /* Reset the RCC clock configuration to the default reset state(for debug purpose) */ /* Set HSION bit */ RCC->CR |= (uint32_t)0x00000001; /* Reset SW, HPRE, PPRE1, PPRE2, ADCPRE and MCO bits */ #ifndef STM32F10X_CL RCC->CFGR &= (uint32_t)0xF8FF0000; #else RCC->CFGR &= (uint32_t)0xF0FF0000; #endif /* STM32F10X_CL */ /* Reset HSEON, CSSON and PLLON bits */ RCC->CR &= (uint32_t)0xFEF6FFFF; /* Reset HSEBYP bit */ RCC->CR &= (uint32_t)0xFFFBFFFF; /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE/OTGFSPRE bits */ RCC->CFGR &= (uint32_t)0xFF80FFFF; #ifdef STM32F10X_CL /* Reset PLL2ON and PLL3ON bits */ RCC->CR &= (uint32_t)0xEBFFFFFF; /* Disable all interrupts and clear pending bits */ RCC->CIR = 0x00FF0000; /* Reset CFGR2 register */ RCC->CFGR2 = 0x00000000; #elif defined (STM32F10X_LD_VL) || defined (STM32F10X_MD_VL) || (defined STM32F10X_HD_VL) /* Disable all interrupts and clear pending bits */ RCC->CIR = 0x009F0000; /* Reset CFGR2 register */ RCC->CFGR2 = 0x00000000; #else /* Disable all interrupts and clear pending bits */ RCC->CIR = 0x009F0000; #endif /* STM32F10X_CL */ #if defined (STM32F10X_HD) || (defined STM32F10X_XL) || (defined STM32F10X_HD_VL) #ifdef DATA_IN_ExtSRAM SystemInit_ExtMemCtl(); #endif /* DATA_IN_ExtSRAM */ #endif /* Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ /* Configure the Flash Latency cycles and enable prefetch buffer */ SetSysClock(); #ifdef VECT_TAB_SRAM SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ #else SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ #endif }
// MCU STOP mode void deepsleep(void) { // Enable PWR clock RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); // Enter Stop Mode PWR_EnterSTOPMode(PWR_Regulator_LowPower, PWR_STOPEntry_WFI); // After wake-up from STOP reconfigure the PLL SetSysClock(); }
/** * @brief Main program. * @param None * @retval None */ int main(void) { /* Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ SetSysClock(); /* This function fills the RCC_ClockFreq structure with the current frequencies of different on chip clocks (for debug purpose) */ RCC_GetClocksFreq(&RCC_ClockFreq); /* Enable Clock Security System(CSS): this will generate an NMI exception when HSE clock fails */ RCC_ClockSecuritySystemCmd(ENABLE); /* NVIC configuration ------------------------------------------------------*/ NVIC_Configuration(); /* Initialize Leds mounted on STM3210X-EVAL board --------------------------*/ STM_EVAL_LEDInit(LED1); STM_EVAL_LEDInit(LED2); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); /* Output HSE clock on MCO pin ---------------------------------------------*/ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); RCC_MCOConfig(RCC_MCO_HSE); while (1) { /* Toggle LED1 */ STM_EVAL_LEDToggle(LED1); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED2 */ STM_EVAL_LEDToggle(LED2); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED3 */ STM_EVAL_LEDToggle(LED3); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED4 */ STM_EVAL_LEDToggle(LED4); /* Insert a delay */ Delay(DELAY_COUNT); } }
/** * @brief Restore peripheral config before entering STOP mode. * @param None * @retval None */ void RestoreConfiguration(void) { /* Restore system clock to 32MHz */ SetSysClock(); /* Enable HSI clock for ADC */ RCC_HSICmd(ENABLE); /* LSI Enable */ RCC_LSICmd(ENABLE); }
void deepsleep(void) { // Request to enter STOP mode with regulator in low power mode HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI); // After wake-up from STOP need to reconfigure the system clock SetSysClock(); #if DEVICE_LOWPOWERTIMER rtc_synchronize(); #endif }
int main(void) { // Configure clock, this figures out HSE for hardware autodetect SetSysClock(0); systemInit(); Serial1 = uartOpen(USART1, &receive_cb, 115200, MODE_RXTX); init_printf( NULL, _putc); while (1); }
void deepsleep(void) { // Disable HAL tick interrupt TimMasterHandle.Instance = TIM5; __HAL_TIM_DISABLE_IT(&TimMasterHandle, TIM_IT_CC2); // Request to enter STOP mode with regulator in low power mode HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI); // After wake-up from STOP reconfigure the PLL SetSysClock(); // Enable HAL tick interrupt __HAL_TIM_ENABLE_IT(&TimMasterHandle, TIM_IT_CC2); }
int main(void) { void SetSysClock(bool overclock); void systemInit(void); // Configure clock, this figures out HSE for hardware autodetect SetSysClock(0); systemInit(); setup(); while (1) { loop(); } }
/***************************************************************************//** * @brief RCC example.This example shows how to configure the System clock(SYSCLK) * to have different frequencies: 24MHz, 36MHz, 48MHz, 56MHz and 72MHz. * You will see the LED PB8 PB9 PB10 blink successively. ******************************************************************************/ void RCC_Init(void) { /* Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ SetSysClock(); /* This function fills the RCC_ClockFreq structure with the current frequencies of different on chip clocks (for debug purpose) */ RCC_GetClocksFreq(&RCC_ClockFreq); /* Enable Clock Security System(CSS): this will generate an NMI exception when HSE clock fails */ RCC_ClockSecuritySystemCmd(ENABLE); /* NVIC configuration ------------------------------------------------------*/ NVIC_Configuration(); }
/** * @brief Setup the microcontroller system * Initialize the Embedded Flash Interface, the PLL and update the SystemFrequency variable. * @note This function should be used only after reset. * @param None * @retval None */ void SystemInit (void) { /* Reset the RCC clock configuration to the default reset state(for debug purpose) */ /* Set HSION bit */ RCC->CR |= (uint32_t)0x00000001; /* Reset SW, HPRE, PPRE1, PPRE2, ADCPRE and MCO bits */ #ifndef STM32F10X_CL RCC->CFGR &= (uint32_t)0xF8FF0000; #else RCC->CFGR &= (uint32_t)0xF0FF0000; #endif /* STM32F10X_CL */ /* Reset HSEON, CSSON and PLLON bits */ RCC->CR &= (uint32_t)0xFEF6FFFF; /* Reset HSEBYP bit */ RCC->CR &= (uint32_t)0xFFFBFFFF; /* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE/OTGFSPRE bits */ RCC->CFGR &= (uint32_t)0xFF80FFFF; #ifndef STM32F10X_CL /* Disable all interrupts and clear pending bits */ RCC->CIR = 0x009F0000; #else /* Reset PLL2ON and PLL3ON bits */ RCC->CR &= (uint32_t)0xEBFFFFFF; /* Disable all interrupts and clear pending bits */ RCC->CIR = 0x00FF0000; /* Reset CFGR2 register */ RCC->CFGR2 = 0x00000000; #endif /* STM32F10X_CL */ /* Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ /* Configure the Flash Latency cycles and enable prefetch buffer */ SetSysClock(); #ifdef STM32F10X_HD #ifdef DATA_IN_ExtSRAM SystemInit_ExtMemCtl(); #endif /* DATA_IN_ExtSRAM */ #endif /* STM32F10X_HD */ }
void systemInit(void) { checkForBootLoaderRequest(); SetSysClock(false); #ifdef CC3D /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ extern void *isr_vector_table_base; NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0); #endif // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // Turn on clocks for stuff we use RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); // cache RCC->CSR value to use it in isMPUSoftreset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); enableGPIOPowerUsageAndNoiseReductions(); // Set USART1 TX (PA9) to output and high state to prevent a rs232 break condition on reset. // See issue https://github.com/cleanflight/cleanflight/issues/1433 gpio_config_t gpio; gpio.mode = Mode_Out_PP; gpio.speed = Speed_2MHz; gpio.pin = Pin_9; digitalHi(GPIOA, gpio.pin); gpioInit(GPIOA, &gpio); // Turn off JTAG port 'cause we're using the GPIO for leds #define AFIO_MAPR_SWJ_CFG_NO_JTAG_SW (0x2 << 24) AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW; // Init cycle counter cycleCounterInit(); memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); }
void SystemInit(void) { /* FPU settings ------------------------------------------------------------*/ #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ #endif /* Reset the RCC clock configuration to the default reset state ------------*/ /* Set MSION bit */ RCC->CR |= RCC_CR_MSION; /* Reset CFGR register */ RCC->CFGR = 0x00000000; /* Reset HSEON, CSSON , HSION, and PLLON bits */ RCC->CR &= (uint32_t)0xEAF6FFFF; /* Reset PLLCFGR register */ RCC->PLLCFGR = 0x00000800; /* Reset HSEBYP bit */ RCC->CR &= (uint32_t)0xFFFBFFFF; /* Disable all interrupts */ RCC->CIER = 0x00000000; /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #else SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif /* Configure the Cube driver */ SystemCoreClock = MSI_VALUE; // At this stage the MSI is used as system clock HAL_Init(); /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings */ SetSysClock(); /* Reset the timer to avoid issues after the RAM initialization */ TIM_MST_RESET_ON; TIM_MST_RESET_OFF; }
/** * @brief Setup the microcontroller system * Initialize the Embedded Flash Interface, initialize the PLL and update th SystemFrequency variable * @param None. * @arg None. * @note : This function should be used only after reset. * @retval value: None. */ void SystemInit(void) { /*!< RCC system reset(for debug purpose) */ /*!< Set HSION bit */ RCC->CR |= (uint32_t)0x00000001; /*!< Reset SW[1:0], HPRE[3:0], PPRE1[2:0], PPRE2[2:0], ADCPRE[1:0] and MCO[2:0] bits */ RCC->CFGR &= (uint32_t)0xF8FF0000; /*!< Reset HSEON, CSSON and PLLON bits */ RCC->CR &= (uint32_t)0xFEF6FFFF; /*!< Reset HSEBYP bit */ RCC->CR &= (uint32_t)0xFFFBFFFF; /*!< Reset PLLSRC, PLLXTPRE, PLLMUL[3:0] and USBPRE bits */ RCC->CFGR &= (uint32_t)0xFF80FFFF; /*!< Disable all interrupts */ RCC->CIR = 0x00000000; /*!< Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ /*!< Configure the Flash Latency cycles and enable prefetch buffer */ SetSysClock(); }
void deepsleep(void) { // Stop HAL systick HAL_SuspendTick(); // Request to enter STOP mode with regulator in low power mode #if TARGET_STM32L4 HAL_PWREx_EnterSTOP2Mode(PWR_STOPENTRY_WFI); #else /* TARGET_STM32L4 */ HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI); #endif /* TARGET_STM32L4 */ // Restart HAL systick HAL_ResumeTick(); // After wake-up from STOP reconfigure the PLL SetSysClock(); #if DEVICE_LOWPOWERTIMER rtc_synchronize(); #endif }
int main(void) { //spiInit(SPIDEV_1); // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); SetSysClock(); systemInit(); timerInit(); // timer must be initialized before any channel is allocated serial0 = serial0_open(); dmaInit(); setup(); while (true) { #ifndef EXTERNAL_DEBUG static uint32_t dbg_start_msec; // support reboot from host computer if (millis()-dbg_start_msec > 100) { dbg_start_msec = millis(); while (serialRxBytesWaiting(serial0)) { uint8_t c = serialRead(serial0); if (c == 'R') systemResetToBootloader(); } } #endif loop(); } } // main
void systemInit(void) { checkForBootLoaderRequest(); // Enable FPU SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); SetSysClock(); // Configure NVIC preempt/priority groups NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // cache RCC->CSR value to use it in isMPUSoftreset() and others cachedRccCsrValue = RCC->CSR; RCC_ClearFlag(); enableGPIOPowerUsageAndNoiseReductions(); // Init cycle counter cycleCounterInit(); memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); }
void init(void) { drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif i2cSetOverclock(masterConfig.i2c_highspeed); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef BUTTONS buttonsInit(); if (!isMPUSoftReset()) { buttonsHandleColdBootButtonPresses(); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; if (feature(FEATURE_SONAR)) { sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); sonarGPIOConfig_t sonarGPIOConfig = { .gpio = SONAR_GPIO, .triggerPin = sonarHardware->echo_pin, .echoPin = sonarHardware->trigger_pin, }; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2); #endif #if defined(USE_UART3) pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3); #endif #if defined(USE_UART4) pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4); #endif #if defined(USE_UART5) pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params); mixerUsePWMIOConfiguration(pwmIOConfiguration); debug[2] = pwmIOConfiguration->pwmInputCount; debug[3] = pwmIOConfiguration->ppmInputCount; if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_UART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); #ifdef USE_SERVOS mixerInitialiseServoFiltering(targetLooptime); #endif #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); /* Setup scheduler */ if (masterConfig.gyroSync) { rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME); } else { rescheduleTask(TASK_GYROPID, targetLooptime); } setTaskEnabled(TASK_GYROPID, true); setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif while (1) { scheduler(); processLoopback(); } } void HardFault_Handler(void) { // fall out of the sky uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredStateForMotors) == requiredStateForMotors) { stopMotors(); } #ifdef TRANSPONDER // prevent IR LEDs from burning out. uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED; if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) { transponderIrDisable(); } #endif while (1); }
void systemInit(bool overclock) { //RCC_ClocksTypeDef rccClocks; int i; // start fpu SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); /* Reset the RCC clock configuration to the default reset state ------------*/ /* Set HSION bit */ RCC->CR |= (uint32_t)0x00000001; /* Reset CFGR register */ RCC->CFGR = 0x00000000; /* Reset HSEON, CSSON and PLLON bits */ RCC->CR &= (uint32_t)0xFEF6FFFF; /* Reset PLLCFGR register */ RCC->PLLCFGR = 0x24003010; /* Reset HSEBYP bit */ RCC->CR &= (uint32_t)0xFFFBFFFF; /* Disable all interrupts */ RCC->CIR = 0x00000000; #ifdef DATA_IN_ExtSRAM SystemInit_ExtMemCtl(); #endif /* DATA_IN_ExtSRAM */ /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ SetSysClock(); #define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field. /* Configure the Vector Table location add offset address ------------------*/ #ifdef VECT_TAB_SRAM SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ #else SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ #endif // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); // RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz // Turn on peripherial clocks // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_ADC12, ENABLE); // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); // USART1, USART2 // RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); // ADC2 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); // RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM Out + PWM RX RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM Out RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM Out + PWM RX RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C3, ENABLE); // i2c RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); // RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); // // RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // // // PPM + PWM RX // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); // // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM Out // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // PWM Out // RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // PWM Out // RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry // RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); // GPS // RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX RCC_ClearFlag(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority gpioStart(); spiInit(); // if feature(FEATURE_I2C) // i2cInit(I2C2); for (i = 0; i < 10; i++) { LED0_TOGGLE delay(25); BEEP_ON delay(25); BEEP_OFF } LED0_OFF }
void init(void) { printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; // initialize IO (needed for all IO operations) IOInitGlobal(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif i2cSetOverclock(masterConfig.i2c_overclock); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(500); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR if (feature(FEATURE_SONAR)) { const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); if (sonarHardware) { pwm_params.useSonar = true; pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; } } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef USE_SERVOS pwm_params.useServos = isServoOutputEnabled(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmInit(&pwm_params); mixerUsePWMIOConfiguration(); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .ioTag = IO_TAG(BEEPER), #ifdef BEEPER_INVERTED .isOD = false, .isInverted = true #else .isOD = true, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.isOD = false; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef GPS if (feature(FEATURE_GPS)) { gpsPreInit(&masterConfig.gpsConfig); } #endif // Set gyro sampling rate divider before initialization gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator); if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); } #endif #ifdef NAV navigationInit( &masterConfig.navConfig, ¤tProfile->pidProfile, ¤tProfile->rcControlsConfig, &masterConfig.rxConfig, &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig ); #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); /* Setup scheduler */ schedulerInit(); rescheduleTask(TASK_GYROPID, targetLooptime); setTaskEnabled(TASK_GYROPID, true); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif while (true) { scheduler(); processLoopback(); } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; bool sensorsOK = false; #ifdef SOFTSERIAL_LOOPBACK serialPort_t *loopbackPort1 = NULL; serialPort_t *loopbackPort2 = NULL; #endif initEEPROM(); checkFirstTime(false); readEEPROM(); // Configure clock, this figures out HSE for hardware autodetect SetSysClock(mcfg.emf_avoidance); // determine hardware revision if (hse_value == 8000000) hw_revision = NAZE32; else if (hse_value == 12000000) hw_revision = NAZE32_REV5; systemInit(); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(); break; } } // sleep for 100ms delay(100); activateConfig(); #ifndef CJMCU if (spiInit() == SPI_DEVICE_MPU && hw_revision == NAZE32_REV5) hw_revision = NAZE32_SP; #endif if (hw_revision != NAZE32_SP) i2cInit(I2C_DEVICE); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9 || mcfg.power_adc_channel == 5)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } // configure rssi ADC if (mcfg.rssi_adc_channel > 0 && (mcfg.rssi_adc_channel == 1 || mcfg.rssi_adc_channel == 9 || mcfg.rssi_adc_channel == 5) && mcfg.rssi_adc_channel != mcfg.power_adc_channel) adc_params.rssiAdcChannel = mcfg.rssi_adc_channel; else { adc_params.rssiAdcChannel = 0; mcfg.rssi_adc_channel = 0; } adcInit(&adc_params); // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); initBoardAlignment(); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(); // production debug output #ifdef PROD_DEBUG productionDebug(); #endif // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; imuInit(); // Mag is initialized inside imuInit mixerInit(); // this will set core.useServo var depending on mixer type serialInit(mcfg.serial_baudrate); // when using airplane/wing mixer, servo/motor outputs are remapped if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE) pwm_params.airplane = true; else pwm_params.airplane = false; pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum pwm_params.useServos = core.useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.pwmFilter = mcfg.pwm_filter; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.syncPWM = feature(FEATURE_SYNCPWM); pwm_params.fastPWM = feature(FEATURE_FASTPWM); pwm_params.servoCenterPulse = mcfg.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); core.numServos = pwm_params.numServos; // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: sbusInit(&rcReadRawFunc); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); break; case SERIALRX_MSP: mspInit(&rcReadRawFunc); break; case SERIALRX_IBUS: ibusInit(&rcReadRawFunc); break; } } #ifndef CJMCU // Optional GPS - available in both PPM, PWM and serialRX input mode, in PWM input, reduces number of available channels by 2. // gpsInit will return if FEATURE_GPS is not enabled. gpsInit(mcfg.gps_baudrate); #endif #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif #ifndef CJMCU if (feature(FEATURE_SOFTSERIAL)) { //mcfg.softserial_baudrate = 19200; // Uncomment to override config value setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted); setupSoftSerialSecondary(mcfg.softserial_2_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort1 = (serialPort_t *)(&softSerialPorts[0])); serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n"); loopbackPort2 = (serialPort_t *)(&softSerialPorts[1])); serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n"); #endif //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial. } if (feature(FEATURE_TELEMETRY)) initTelemetry(); #endif previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLE = 1; // loopy while (1) { loop(); #ifdef SOFTSERIAL_LOOPBACK if (loopbackPort1) { while (serialTotalBytesWaiting(loopbackPort1)) { uint8_t b = serialRead(loopbackPort1); serialWrite(loopbackPort1, b); //serialWrite(core.mainport, 0x01); //serialWrite(core.mainport, b); }; } if (loopbackPort2) { while (serialTotalBytesWaiting(loopbackPort2)) { serialRead(loopbackPort2); }; } #endif } }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef STM32F40_41xxx SetSysClock(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); ledInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; if (feature(FEATURE_SONAR)) { sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); sonarGPIOConfig_t sonarGPIOConfig = { .gpio = SONAR_GPIO, .triggerPin = sonarHardware->echo_pin, .echoPin = sonarHardware->trigger_pin, }; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif #if defined(USE_USART2) && defined(STM32F40_41xxx) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #if defined(USE_USART6) && defined(STM32F40_41xxx) pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); spiInit(SPI3); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE_INT); #if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(SPARKY2) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { #ifdef I2C_DEVICE_EXT i2cInit(I2C_DEVICE_EXT); #endif } #endif #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { #ifdef COLIBRI if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) { ledStripEnable(); } #else ledStripEnable(); #endif } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef BLACKBOX initBlackbox(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); while (1) { loop(); processLoopback(); } } void HardFault_Handler(void) { // fall out of the sky uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredState) == requiredState) { stopMotors(); } while (1); }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; bool sensorsOK = false; initPrintfSupport(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef NAZE detectHardwareRevision(); #endif systemInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioMode = Mode_Out_OD, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, .isInverted = false }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef NAZE updateHardwareRevision(); #endif #ifdef USE_I2C #ifdef NAZE if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #else // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif #if !defined(SPARKY) drv_adc_config_t adc_params; adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination); // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif serialInit(&masterConfig.serialConfig); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(SERIAL_PORT_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); failsafe = failsafeInit(&masterConfig.rxConfig); beepcodeInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { Sonar_init(); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) initTelemetry(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayEnablePageCycling(); #endif } #endif } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); while (1) { loop(); processLoopback(); } }
/** * @brief Main program. * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f10x.c file */ /* Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers */ SetSysClock(); /* This function fills the RCC_ClockFreq structure with the current frequencies of different on chip clocks (for debug purpose) */ RCC_GetClocksFreq(&RCC_ClockFreq); /* Enable Clock Security System(CSS): this will generate an NMI exception when HSE clock fails */ RCC_ClockSecuritySystemCmd(ENABLE); /* NVIC configuration ------------------------------------------------------*/ NVIC_Configuration(); /* Initialize Leds mounted on STM3210X-EVAL board --------------------------*/ STM_EVAL_LEDInit(LED1); STM_EVAL_LEDInit(LED2); STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); /* Output HSE clock on MCO pin ---------------------------------------------*/ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); RCC_MCOConfig(RCC_MCO_HSE); while (1) { /* Toggle LED1 */ STM_EVAL_LEDToggle(LED1); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED2 */ STM_EVAL_LEDToggle(LED2); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED3 */ STM_EVAL_LEDToggle(LED3); /* Insert delay */ Delay(DELAY_COUNT); /* Toggle LED4 */ STM_EVAL_LEDToggle(LED4); /* Insert a delay */ Delay(DELAY_COUNT); } }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef STM32F40_41xxx SetSysClock(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); ledInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif #if defined(USE_USART2) && defined(STM32F40_41xxx) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #if defined(USE_USART6) && defined(STM32F40_41xxx) pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); spiInit(SPI3); spiInit(SPI4); spiInit(SPI5); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else #if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(STM32F4DISCOVERY) i2cInit(I2C_DEVICE_INT); if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { #ifdef I2C_DEVICE_EXT i2cInit(I2C_DEVICE_EXT); #endif } #endif #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(3); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig); failsafeInit(&masterConfig.rxConfig); rxInit(&masterConfig.rxConfig); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(&masterConfig.batteryConfig); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { #ifdef COLIBRI if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) { ledStripEnable(); } #else ledStripEnable(); #endif } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #endif #if defined(SPRACINGF3) || defined(CC3D) || defined(COLIBRI) || defined(REVO) m25p16_init(); #endif flashfsInit(); #endif #ifdef BLACKBOX //initBlackbox(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } //gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet //timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif #include <stdio.h> #include "stm32f4xx_rcc.h" #include "stm32f4xx_gpio.h" GPIO_InitTypeDef GPIO_InitStruct; int main(void) { RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13 | GPIO_Pin_12; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOD, &GPIO_InitStruct); printf("Hello World!\r\n"); hello(); while (1) { static int count = 0; static int i; for (i = 0; i < 10000000; ++i) ; GPIO_ToggleBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); printf("%d\r\n", ++count); } //init(); /* while (1) { //loop(); int x = 1;//processLoopback(); }*/ }
void init(void) { drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(systemConfig()->emf_avoidance); #endif i2cSetOverclock(systemConfig()->i2c_highspeed); systemInit(); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = debugConfig()->debug_mode; #ifdef USE_EXTI EXTIInit(); #endif #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); } else { ledInit(true); } #else ledInit(false); #endif #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef BUTTONS buttonsInit(); if (!isMPUSoftReset()) { buttonsHandleColdBootButtonPresses(); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(rxConfig()); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); serialInit(feature(FEATURE_SOFTSERIAL)); mixerInit(customMotorMixer(0)); #ifdef USE_SERVOS mixerInitServos(customServoMixer(0)); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; sonarGPIOConfig_t sonarGPIOConfig; if (feature(FEATURE_SONAR)) { bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC); sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins); sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio; sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin; sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio; sonarGPIOConfig.echoPin = sonarHardware->echo_pin; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2); #endif #if defined(USE_UART3) pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3); #endif #if defined(USE_UART4) pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4); #endif #if defined(USE_UART5) pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = ( feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC ); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate; pwm_params.idlePulse = calculateMotorOff(); if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(); // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params); mixerUsePWMIOConfiguration(pwmIOConfiguration); #ifdef DEBUG_PWM_CONFIGURATION debug[2] = pwmIOConfiguration->pwmInputCount; debug[3] = pwmIOConfiguration->ppmInputCount; #endif if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #ifdef STM32F303xC #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPI3); } #else spiInit(SPI3); #endif #endif #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_UART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.channelMask = 0; #ifdef ADC_BATTERY adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0); #endif #ifdef ADC_RSSI adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0); #endif #ifdef ADC_AMPERAGE adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0); #endif #ifdef ADC_POWER_12V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V); #endif #ifdef ADC_POWER_5V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V); #endif #ifdef ADC_POWER_3V adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V); #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0; #endif adcInit(&adc_params); #endif initBoardAlignment(); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(); } #endif #ifdef NAZE if (hardwareRevision < NAZE32_REV5) { gyroConfig()->gyro_sync = 0; } #endif if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); mspInit(); mspSerialInit(); const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom); pidInitFilters(pidProfile()); #ifdef USE_SERVOS mixerInitialiseServoFiltering(targetPidLooptime); #endif imuInit(); #ifdef USE_CLI cliInit(); #endif failsafeInit(); rxInit(modeActivationProfile()->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit(); navigationInit(pidProfile()); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(transponderConfig()->data); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif if (mixerConfig()->mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif if (feature(FEATURE_VBAT)) { // Now that everything has powered up the voltage and cell count be determined. voltageMeterInit(); batteryInit(); } if (feature(FEATURE_AMPERAGE_METER)) { amperageMeterInit(); } #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif void configureScheduler(void) { schedulerInit(); setTaskEnabled(TASK_SYSTEM, true); uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz); rescheduleTask(TASK_GYRO, gyroPeriodUs); setTaskEnabled(TASK_GYRO, true); rescheduleTask(TASK_PID, gyroPeriodUs); setTaskEnabled(TASK_PID, true); if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif }