/*
* Called to activate/deactivate beeper, using the given "BEEPER_..." value.
* This function returns immediately (does not block).
*/
void beeper(beeperMode_e mode)
{
if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount < 2)))) {
beeperSilence();
return;
}
const beeperTableEntry_t *selectedCandidate = NULL;
for (uint32_t i = 0; i < BEEPER_TABLE_ENTRY_COUNT; i++) {
const beeperTableEntry_t *candidate = &beeperTable[i];
if (candidate->mode != mode) {
continue;
}
if (!currentBeeperEntry) {
selectedCandidate = candidate;
break;
}
if (candidate->priority < currentBeeperEntry->priority) {
selectedCandidate = candidate;
}
break;
}
if (!selectedCandidate) {
return;
}
currentBeeperEntry = selectedCandidate;
beeperPos = 0;
beeperNextToggleTime = 0;
}
/*
* Beeper handler function to be called periodically in loop. Updates beeper
* state via time schedule.
*/
void beeperUpdate(timeUs_t currentTimeUs)
{
// If beeper option from AUX switch has been selected
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
beeper(BEEPER_RX_SET);
#ifdef USE_GPS
} else if (feature(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) {
beeperGpsStatus();
#endif
}
// Beeper routine doesn't need to update if there aren't any sounds ongoing
if (currentBeeperEntry == NULL) {
return;
}
if (beeperNextToggleTime > currentTimeUs) {
return;
}
if (!beeperIsOn) {
beeperIsOn = 1;
#ifdef USE_DSHOT
if (!areMotorsRunning() && beeperConfig()->dshotBeaconTone && (beeperConfig()->dshotBeaconTone <= DSHOT_CMD_BEACON5) && (currentBeeperEntry->mode == BEEPER_RX_SET || currentBeeperEntry->mode == BEEPER_RX_LOST)) {
pwmDisableMotors();
delay(1);
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone);
pwmEnableMotors();
}
#endif
if (currentBeeperEntry->sequence[beeperPos] != 0) {
if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1))))
BEEP_ON;
warningLedEnable();
warningLedRefresh();
// if this was arming beep then mark time (for blackbox)
if (
beeperPos == 0
&& (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX)
) {
armingBeepTimeMicros = micros();
}
}
} else {
beeperIsOn = 0;
if (currentBeeperEntry->sequence[beeperPos] != 0) {
BEEP_OFF;
warningLedDisable();
warningLedRefresh();
}
}
beeperProcessCommand(currentTimeUs);
}
/*
* Beeper handler function to be called periodically in loop. Updates beeper
* state via time schedule.
*/
void beeperUpdate(void)
{
// If beeper option from AUX switch has been selected
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
#ifdef GPS
if (feature(FEATURE_GPS)) {
beeperGpsStatus();
} else {
beeper(BEEPER_RX_SET);
}
#else
beeper(BEEPER_RX_SET);
#endif
}
// Beeper routine doesn't need to update if there aren't any sounds ongoing
if (currentBeeperEntry == NULL) {
return;
}
uint32_t now = millis();
if (beeperNextToggleTime > now) {
return;
}
if (!beeperIsOn) {
beeperIsOn = 1;
if (currentBeeperEntry->sequence[beeperPos] != 0) {
if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1))))
BEEP_ON;
warningLedEnable();
warningLedRefresh();
// if this was arming beep then mark time (for blackbox)
if (
beeperPos == 0
&& (currentBeeperEntry->mode == BEEPER_ARMING || currentBeeperEntry->mode == BEEPER_ARMING_GPS_FIX)
) {
armingBeepTimeMicros = micros();
}
}
} else {
beeperIsOn = 0;
if (currentBeeperEntry->sequence[beeperPos] != 0) {
BEEP_OFF;
warningLedDisable();
warningLedRefresh();
}
}
beeperProcessCommand();
}
static void beeperGpsStatus(void)
{
if (!(getBeeperOffMask() & (1 << (BEEPER_GPS_STATUS - 1)))) {
// if GPS fix then beep out number of satellites
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
uint8_t i = 0;
do {
beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10;
} while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
beep_multiBeeps[i - 1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
beeper(BEEPER_MULTI_BEEPS); //initiate sequence
}
}
}
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)
{
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
}
