/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(~RCC_AHBRSTR_FLITFRST);
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, TRUE);
/* SysTick initialization using the system clock.*/
SysTick->LOAD = Clk.AHBFreqHz / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
/* DWT cycle counter enable.*/
// SCS_DEMCR |= SCS_DEMCR_TRCENA;
// DWT_CTRL |= DWT_CTRL_CYCCNTENA;
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
// hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.
Note, GPIOs are not reset because initialized before this point in
board files.*/
rccResetAHB(~STM32_GPIO_EN_MASK);
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* PWR clock enabled.*/
rccEnablePWRInterface(true);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
/* DMA subsystems initialization.*/
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* IRQ subsystem initialization.*/
irqInit();
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
/* SYSCFG clock enabled here because it is a multi-functional unit shared
among multiple drivers.*/
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, true);
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(~RCC_AHBRSTR_FLITFRST);
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
// @KL
/* SYSCFG clock enabled here because it is a multi-functional unit shared
among multiple drivers.*/
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, TRUE);
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals. AHB3 is not reseted because it could have
been initialized in the board initialization file (board.c).*/
rccResetAHB1(~0);
rccResetAHB2(~0);
rccResetAHB3(~0);
rccResetAPB1R1(~RCC_APB1RSTR1_PWRRST);
rccResetAPB1R2(~0);
rccResetAPB2(~0);
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR1 |= PWR_CR1_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(~RCC_APB2RSTR_DBGMCURST);
/* SysTick initialization using the system clock.*/
SysTick->LOAD = Clk.AHBFreqHz / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* PWR and BD clocks enabled.*/
rccEnablePWRInterface(true);
rccEnableBKPInterface(true);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
/* DMA subsystems initialization.*/
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* IRQ subsystem initialization.*/
irqInit();
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(0xFFFFFFFF);
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
/* SYSCFG clock enabled here because it is a multi-functional unit shared
among multiple drivers.*/
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, TRUE);
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals. AHB3 is not reseted because it could have
been initialized in the board initialization file (board.c).*/
rccResetAHB1(~0);
rccResetAHB2(~0);
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
/* SysTick initialization using the system clock.*/
SysTick->LOAD = STM32_HCLK / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
/* DWT cycle counter enable.*/
SCS_DEMCR |= SCS_DEMCR_TRCENA;
DWT_CTRL |= DWT_CTRL_CYCCNTENA;
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals. AHB3 is not reseted because it could have
been initialized in the board initialization file (board.c).
Note, GPIOs are not reset because initialized before this point in
board files.*/
rccResetAHB1(~STM32_GPIO_EN_MASK);
#if !defined(STM32F410xx)
rccResetAHB2(~0);
#endif
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
/* PWR clock enabled.*/
rccEnablePWRInterface(true);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
/* DMA subsystems initialization.*/
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* IRQ subsystem initialization.*/
irqInit();
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
THD_TABLE_END
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
WDTCTL = WDTPW | WDTHOLD;
halInit();
chSysInit();
dmaInit();
/* This is now the idle thread loop, you may perform here a low priority
task but you must never try to sleep or wait in this loop. Note that
this tasks runs at the lowest priority level so any instruction added
here will be executed after all other tasks have been started.*/
while (true) {
}
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* SysTick initialization using the system clock.*/
SysTick->LOAD = STM32_HCLK / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
/* DWT cycle counter enable.*/
SCS_DEMCR |= SCS_DEMCR_TRCENA;
DWT_CTRL |= DWT_CTRL_CYCCNTENA;
/* PWR and BD clocks enabled.*/
rccEnablePWRInterface(FALSE);
rccEnableBKPInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(0xFFFFFFFF);
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
/* DMA subsystems initialization.*/
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* IRQ subsystem initialization.*/
irqInit();
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
/* SYSCFG clock enabled here because it is a multi-functional unit shared
among multiple drivers.*/
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, TRUE);
#if STM32_HAS_USB
/* USB IRQ relocated to not conflict with CAN.*/
SYSCFG->CFGR1 |= SYSCFG_CFGR1_USB_IT_RMP;
#endif
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals except those on IOP.*/
rccResetAHB(~RCC_AHBRSTR_MIFRST);
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
/* PWR clock enabled.*/
rccEnablePWRInterface(true);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
/* DMA subsystems initialization.*/
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* IRQ subsystem initialization.*/
irqInit();
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
/**
* @brief Low level HAL driver initialization.
*/
void hal_lld_init(void) {
/* SysTick initialization using the system clock.*/
SysTick->LOAD = STM32_HCLK / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
#if CH_HAL_USE_ADC || CH_HAL_USE_SPI
dmaInit();
#endif
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Disabling PMC write protection. */
pmcDisableWP();
/* Enabling matrix clock */
pmcEnableH32MX();
pmcEnableH64MX();
/* Enabling write protection. */
pmcEnableWP();
#if defined(SAMA_DMA_REQUIRED)
dmaInit();
#endif
/* Advanced interrupt controller init */
aicInit();
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(0xFFFFFFFF);
rccResetAPB1(0xFFFFFFFF);
rccResetAPB2(0xFFFFFFFF);
/* SysTick initialization using the system clock.*/
SysTick->LOAD = STM32_HCLK / CH_FREQUENCY - 1;
SysTick->VAL = 0;
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
SysTick_CTRL_ENABLE_Msk |
SysTick_CTRL_TICKINT_Msk;
/* DWT cycle counter enable.*/
SCS_DEMCR |= SCS_DEMCR_TRCENA;
DWT_CTRL |= DWT_CTRL_CYCCNTENA;
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
/* SYSCFG clock enabled here because it is a multi-functional unit shared
among multiple drivers.*/
rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, TRUE);
/* USB IRQ relocated to not conflict with CAN.*/
SYSCFG->CFGR1 |= SYSCFG_CFGR1_USB_IT_RMP;
}
/**
* @brief Low level HAL driver initialization.
*
* @notapi
*/
void hal_lld_init(void) {
/* Reset of all peripherals.*/
rccResetAHB(~RCC_AHBRSTR_FLITFRST);
rccResetAPB1(~RCC_APB1RSTR_PWRRST);
rccResetAPB2(~0);
/* PWR clock enabled.*/
rccEnablePWRInterface(FALSE);
/* Initializes the backup domain.*/
hal_lld_backup_domain_init();
#if defined(STM32_DMA_REQUIRED)
dmaInit();
#endif
/* Programmable voltage detector enable.*/
#if STM32_PVD_ENABLE
PWR->CR |= PWR_CR_PVDE | (STM32_PLS & STM32_PLS_MASK);
#endif /* STM32_PVD_ENABLE */
}
int main(void)
{
CHIP_Init();
/* Enable access to the I2C bus on the DK */
BSP_Init(BSP_INIT_DEFAULT);
BSP_PeripheralAccess(BSP_I2C, true);
/* Set up DMA and I2C */
dmaInit();
i2cInit();
/* Clear error flag. Will be set on any error during transmission. */
i2cError = false;
i2cDmaWrite(EEPROM_I2C_ADDR, 0, (uint8_t *)txData, sizeof(txData));
/* Wait until the I2C transfer is complete */
sleepUntilTransferDone();
/* The EEPROM will be busy for a while after writing to it.
* Do an 'ACK poll' until it is ready */
ackPoll(EEPROM_I2C_ADDR);
/* Read the sequence back from the EEPROM */
i2cDmaRead(EEPROM_I2C_ADDR, 0, (uint8_t *)rxData, sizeof(txData));
/* Wait until the I2C transfer is complete */
sleepUntilTransferDone();
if ( i2cError )
{
/* An error occured during the transfer */
}
/* Done */
while (1);
}
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 init(void)
{
#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// 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
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#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
#if !defined(USE_HAL_DRIVER)
dmaInit();
#endif
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
#endif
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d;
}
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);
}
#endif
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(&masterConfig.pwmConfig);
}
pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
#endif
mixerConfigureOutput();
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
/* temp until PGs are implemented. */
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4);
#endif
#endif
#ifdef VTX
vtxInit();
#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(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#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 USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// 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;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspFcInit();
mspSerialInit();
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#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 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(IO_TAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IO_TAG_NONE);
#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
#if defined(STM32F4) || defined(STM32F7)
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#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;
fcTasksInit();
systemState |= SYSTEM_STATE_READY;
}
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
}
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 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 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);
}
// 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 pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
{
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_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
__DMA1_CLK_ENABLE();
if (configureTimer) {
RCC_ClockCmd(timerRCC(timer), ENABLE);
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;
}
motor->TimHandle.Instance = timerHardware->tim;
motor->TimHandle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;;
motor->TimHandle.Init.Period = MOTOR_BITLENGTH;
motor->TimHandle.Init.RepetitionCounter = 0;
motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_PWM_Init(&motor->TimHandle) != HAL_OK)
{
/* Initialization Error */
return;
}
}
else
{
motor->TimHandle = dmaMotors[timerIndex].TimHandle;
}
switch (timerHardware->channel) {
case TIM_CHANNEL_1:
motor->timerDmaSource = TIM_DMA_ID_CC1;
break;
case TIM_CHANNEL_2:
motor->timerDmaSource = TIM_DMA_ID_CC2;
break;
case TIM_CHANNEL_3:
motor->timerDmaSource = TIM_DMA_ID_CC3;
break;
case TIM_CHANNEL_4:
motor->timerDmaSource = TIM_DMA_ID_CC4;
break;
}
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->dmaStream == NULL)
{
/* Initialization Error */
return;
}
motor->hdma_tim.Instance = timerHardware->dmaStream;
/* 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;
TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
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 adcInit(const adcConfig_t *config)
{
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
uint8_t adcChannelCount = 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;
#ifdef ADC24_DMA_REMAP
SYSCFG_DMAChannelRemapConfig(SYSCFG_DMARemap_ADC2ADC4, ENABLE);
#endif
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_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag);
adcOperatingConfig[i].dmaIndex = adcChannelCount++;
adcOperatingConfig[i].sampleTime = ADC_SampleTime_601Cycles5;
adcOperatingConfig[i].enabled = true;
}
if (!adcActive) {
return;
}
if ((device == ADCDEV_1) || (device == ADCDEV_2)) {
// enable clock for ADC1+2
RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
} else {
// enable clock for ADC3+4
RCC_ADCCLKConfig(RCC_ADC34PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
}
RCC_ClockCmd(adc.rccADC, ENABLE);
dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, 0);
DMA_DeInit(adc.DMAy_Channelx);
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 = 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.DMAy_Channelx, &DMA_InitStructure);
DMA_Cmd(adc.DMAy_Channelx, ENABLE);
// calibrate
ADC_VoltageRegulatorCmd(adc.ADCx, ENABLE);
delay(10);
ADC_SelectCalibrationMode(adc.ADCx, ADC_CalibrationMode_Single);
ADC_StartCalibration(adc.ADCx);
while (ADC_GetCalibrationStatus(adc.ADCx) != RESET);
ADC_VoltageRegulatorCmd(adc.ADCx, 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.ADCx, &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.ADCx, &ADC_InitStructure);
uint8_t rank = 1;
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcOperatingConfig[i].enabled) {
continue;
}
ADC_RegularChannelConfig(adc.ADCx, adcOperatingConfig[i].adcChannel, rank++, adcOperatingConfig[i].sampleTime);
}
ADC_Cmd(adc.ADCx, ENABLE);
while (!ADC_GetFlagStatus(adc.ADCx, ADC_FLAG_RDY));
ADC_DMAConfig(adc.ADCx, ADC_DMAMode_Circular);
ADC_DMACmd(adc.ADCx, ENABLE);
ADC_StartConversion(adc.ADCx);
}
/**
* halAudioHss_init
* initialize and configure the HSS channel and the related media streams
*
* @return
*/
static int halAudioHss_init( void )
{
HSSL_DRV * hssDrvp;
HALAUDIO_TRACE("Hss addon module init called \n");
memset( hssMediaPacketLen, 0, sizeof(int) * HSS_NUM_MEDIA_STREAM );
memset( hssMixerPorts, -1, sizeof( int ) * HSS_NUM_MEDIA_STREAM );
/* enable both HSS channels on the 1103 */
bcm1103mmr->chipCtl.blkEnables |= MMR1103_CHIPCTL_BLKEN_EHSS0 | MMR1103_CHIPCTL_BLKEN_EHSS1;
/* Assign lower driver */
bcm1103HsslDrvInit( &hssCblk.drvp );
hssDrvp = hssCblk.drvp;
/* config the HSS0 channel */
{
hsslReset( hssDrvp,0 ); /* Reset HSS channel */
hsslModeSet( hssDrvp,0, HSSL_MODE_TDM_MASTER ); /* Master mode - do not change */
hsslClkSet( hssDrvp,0, 2048 ); /* Set clock */
hsslClkInvertEnable( hssDrvp,0 ); /* Enabled inverted clock */
hsslLsbFirstDisable( hssDrvp,0 ); /* Select MSB first */
hsslFsLongDisable( hssDrvp,0); /* Long framesync */
hsslFsInvertDisable( hssDrvp,0); /* Framesync inversion */
hsslFsFallingEnable( hssDrvp,0); /* Edge clocking */
hsslFsOffsetSet( hssDrvp,0, 1 ); /* Clock offset */
hsslTimeslotSet( hssDrvp,0, 0 ); /* Time slot to run, 0 indexed */
hsslTimeslotNumSet( hssDrvp,0, 1 ); /* Number of time slots */
hsslSampleFreqSet( hssDrvp,0, 8000 ); /* Frame syn frequency */
hsslSampleSizeSet( hssDrvp,0, 2 ); /* number of byte per sample */
hssCblk.cfg[0].sampleFreq = 8000;
hssCblk.cfg[0].sampleSize = 2;
hssCblk.cfg[0].numslot = 1;
/* HSS channel 0 support 1 media stream, running at 8kHz */
hssMediaPacketLen[0] = 80;
/* define another media stream here if HSS0 support more than 1 media streams */
}
/* config the HSS1 channel */
{
hsslReset( hssDrvp,1 ); /* Reset HSS channel */
hsslModeSet( hssDrvp,1, HSSL_MODE_TDM_MASTER ); /* Master mode - do not change */
hsslClkSet( hssDrvp,1, 2048 ); /* Set clock */
hsslClkInvertEnable( hssDrvp,1 ); /* Enabled inverted clock */
hsslLsbFirstDisable( hssDrvp,1 ); /* Select MSB first */
hsslFsLongDisable( hssDrvp,1); /* Long framesync */
hsslFsInvertDisable( hssDrvp,1); /* Framesync inversion */
hsslFsFallingEnable( hssDrvp,1); /* Edge clocking */
hsslFsOffsetSet( hssDrvp,1, 1 ); /* Clock offset */
hsslTimeslotSet( hssDrvp,1, 0 ); /* Time slot to run, 0 indexed */
hsslTimeslotNumSet( hssDrvp,1, 2 ); /* Number of time slots */
hsslSampleFreqSet( hssDrvp,1, 8000 ); /* Frame syn frequency */
hsslSampleSizeSet( hssDrvp,1, 2); /* Number of byte per sample */
hssCblk.cfg[1].sampleFreq = 8000;
hssCblk.cfg[1].sampleSize = 2;
hssCblk.cfg[1].numslot = 2;
/* HSS channel 1 support 1 media stream, running at 8kHz */
hssMediaPacketLen[1] = 160;
/* define another media stream here if HSS0 support more than 1 media streams */
}
/* registering mixer ports for the media streams */
/* we can register up to 4 mixer ports since up to 4 media streams can be supported */
/* only 2 mixer ports are being registered now since this example HSS code only support 2 media streams */
hssRegisterMixerPorts();
/* initialize dma blocks */
mallocInit();
dmaInit();
/* create proc entry for debug stats */
create_proc_read_entry( AUDIO_HSS_PROC_NAME, 0, NULL, halAudioAddonHssReadProc,
NULL );
create_proc_read_entry( AUDIO_HW_HSS_PROC_NAME, 0, NULL,
halAudioAddonHwHssReadProc, NULL );
/* clear statistics variables */
numIngressCalled = 0;
numEgressCalled = 0;
numIngressError[0] = 0;
numIngressError[1] = 0;
numEgressError[0] = 0;
numEgressError[1] = 0;
return 0;
}
void adcInit(const adcConfig_t *config)
{
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;
const 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_AIN, 0));
adcOperatingConfig[i].adcChannel = adcChannelByTag(adcOperatingConfig[i].tag);
adcOperatingConfig[i].dmaIndex = configuredAdcChannels++;
adcOperatingConfig[i].sampleTime = ADC_SampleTime_239Cycles5;
adcOperatingConfig[i].enabled = true;
}
if (!adcActive) {
return;
}
RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI)
RCC_ClockCmd(adc.rccADC, ENABLE);
dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, 0);
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 (!adcOperatingConfig[i].enabled) {
continue;
}
ADC_RegularChannelConfig(adc.ADCx, adcOperatingConfig[i].adcChannel, rank++, adcOperatingConfig[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 init(void)
{
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// 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
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#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();
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
#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 sonarHardware_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_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_UART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_UART6) && 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 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
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED;
// Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
pwm_params.idlePulse = 0; // brushed motors
}
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
#endif
#ifndef SKIP_RX_PWM_PPM
pwmRxInit(masterConfig.inputFilteringMode);
#endif
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
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
/* temp until PGs are implemented. */
#ifdef BLUEJAYF4
if (hardwareRevision <= BJF4_REV2) {
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
}
#endif
#ifdef CC3D
if (masterConfig.use_buzzer_p6 == 1)
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#endif
#ifdef VTX
vtxInit();
#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(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#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 USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// 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;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) 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, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit();
}
#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 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(IOTAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IOTAG_NONE);
#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
#ifdef STM32F4
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime(gyro.targetLooptime * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#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
void main_init(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
case 500:
case 375:
case 250:
case 125:
accTargetLooptime = 1000;
break;
default:
case 1000:
#ifdef STM32F10X
accTargetLooptime = 1000;
#else
accTargetLooptime = 1000;
#endif
}
rescheduleTask(TASK_ACCEL, accTargetLooptime);
}
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_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(USE_SPI) && 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));
// Reschedule telemetry to 500hz for Jeti Exbus
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000);
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
#ifdef OSD
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
#endif
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
}
