static void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || (averageSystemLoadPercent > 100)) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } }
void annexCode(void) { int32_t throttleValue; // Compute ROLL PITCH and YAW command rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband); //Compute THROTTLE command throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void disarm(void) { if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); lastDisarmTimeUs = micros(); #ifdef USE_BLACKBOX if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON blackboxFinish(); } #endif BEEP_OFF; #ifdef USE_DSHOT if (isMotorProtocolDshot() && flipOverAfterCrashMode && !feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } #endif flipOverAfterCrashMode = false; // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) { beeper(BEEPER_DISARMING); // emit disarm tone } } }
void annexCode(void) { if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
static void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } uint32_t nextBlinkMask = getArmingPreventionBlinkMask(); warningLedSetBlinkMask(nextBlinkMask); warningLedUpdate(); } }
void mwDisarm(void) { if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); } #endif beeper(BEEPER_DISARMING); // emit disarm tone } }
void annexCode(void) { int32_t tmp, tmp2; tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void mwDisarm(void) { armingCalibrationWasInitialised = false; if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); #ifdef BLACKBOX if (blackboxConfig()->device) { finishBlackbox(); } #endif BEEP_OFF; beeper(BEEPER_DISARMING); // emit disarm tone } }
void mwDisarm(void) { armingCalibrationWasInitialised = false; if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); } #endif beeper(BEEPER_DISARMING); // emit disarm tone } }
void updateOsd(uint32_t currentTime) { static uint32_t counter; #ifdef MAX7456_DMA_CHANNEL_TX // don't touch buffers if DMA transaction is in progress if (max7456DmaInProgres()) return; #endif // MAX7456_DMA_CHANNEL_TX // redraw values in buffer if (counter++ % 5 == 0) osdUpdate(currentTime); else // rest of time redraw screen 10 chars per idle to don't lock the main idle max7456DrawScreen(); // do not allow ARM if we are in menu if (inMenu) DISABLE_ARMING_FLAG(OK_TO_ARM); }
void mwDisarm(void) { if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { // the telemetry state must be checked immediately so that shared serial ports are released. checkTelemetryState(); mspAllocateSerialPorts(&masterConfig.serialConfig); } #endif #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { finishBlackbox(); } #endif } }
STATIC_UNIT_TESTED void cmsMenuOpen(void) { if (!cmsInMenu) { // New open pCurrentDisplay = cmsDisplayPortSelectCurrent(); if (!pCurrentDisplay) return; cmsInMenu = true; currentMenu = &menuMain; DISABLE_ARMING_FLAG(OK_TO_ARM); } else { // Switch display displayPort_t *pNextDisplay = cmsDisplayPortSelectNext(); if (pNextDisplay != pCurrentDisplay) { displayRelease(pCurrentDisplay); pCurrentDisplay = pNextDisplay; } else { return; } } displayGrab(pCurrentDisplay); // grab the display for use by the CMS cmsMenuChange(pCurrentDisplay, currentMenu); }
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) { uint8_t i; drv_pwm_config_t pwm_params; bool sensorsOK = false; initPrintfSupport(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef NAZE detectHardwareRevision(); #endif systemInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioMode = Mode_Out_OD, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, .isInverted = false }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef NAZE updateHardwareRevision(); #endif #ifdef USE_I2C #ifdef NAZE if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #else // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif #if !defined(SPARKY) drv_adc_config_t adc_params; adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination); // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif serialInit(&masterConfig.serialConfig); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(SERIAL_PORT_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); failsafe = failsafeInit(&masterConfig.rxConfig); beepcodeInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { Sonar_init(); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) initTelemetry(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayEnablePageCycling(); #endif } #endif } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); while (1) { loop(); processLoopback(); } }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef STM32F40_41xxx SetSysClock(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); ledInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; if (feature(FEATURE_SONAR)) { sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); sonarGPIOConfig_t sonarGPIOConfig = { .gpio = SONAR_GPIO, .triggerPin = sonarHardware->echo_pin, .echoPin = sonarHardware->trigger_pin, }; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif #if defined(USE_USART2) && defined(STM32F40_41xxx) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #if defined(USE_USART6) && defined(STM32F40_41xxx) pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); spiInit(SPI3); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE_INT); #if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(SPARKY2) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { #ifdef I2C_DEVICE_EXT i2cInit(I2C_DEVICE_EXT); #endif } #endif #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { #ifdef COLIBRI if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) { ledStripEnable(); } #else ledStripEnable(); #endif } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef BLACKBOX initBlackbox(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); while (1) { loop(); processLoopback(); } } void HardFault_Handler(void) { // fall out of the sky uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredState) == requiredState) { stopMotors(); } while (1); }
/* * processRx called from taskUpdateRxMain */ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } updateRSSI(currentTimeUs); if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent Iterm from being reset } } else { airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetITerm(); if (currentPidProfile->pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { pidStabilisationState(PID_STABILISATION_ON); } #ifdef USE_RUNAWAY_TAKEOFF // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low. // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable // prevention for the remainder of the battery. if (ARMING_FLAG(ARMED) && pidConfig()->runaway_takeoff_prevention && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled && !STATE(FIXED_WING)) { // Determine if we're in "flight" // - motors running // - throttle over runaway_takeoff_deactivate_throttle_percent // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } } } // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds if (inStableFlight) { if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; } uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay; // at high throttle levels reduce deactivation delay by 50% if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) { deactivateDelay = deactivateDelay / 2; } if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) { runawayTakeoffCheckDisabled = true; } } else { if (runawayTakeoffDeactivateUs != 0) { runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs); } runawayTakeoffDeactivateUs = 0; } if (runawayTakeoffDeactivateUs == 0) { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000); } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000); } } else { DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE); DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE); } #endif // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !feature(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(); #ifdef USE_DSHOT /* Enable beep warning when the crash flip mode is active */ if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { beeper(BEEPER_CRASH_FLIP_MODE); } #endif if (!cliMode) { updateAdjustmentStates(); processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } #ifdef USE_GPS_RESCUE if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) { if (!FLIGHT_MODE(GPS_RESCUE_MODE)) { ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } } else { DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE); } #endif if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; // increase frequency of attitude task to reduce drift when in angle or horizon mode rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500)); } else { LED1_OFF; rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100)); } if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) { DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } #if defined(USE_ACC) || defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { #if defined(USE_GPS) || defined(USE_MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } #endif if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { if (imuQuaternionHeadfreeOffsetSet()){ beeper(BEEPER_RX_SET); } } } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef USE_TELEMETRY static bool sharedPortTelemetryEnabled = false; if (feature(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); telemetryCheckState(); sharedPortTelemetryEnabled = true; } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); sharedPortTelemetryEnabled = false; } } #endif #ifdef USE_VTX_CONTROL vtxUpdateActivatedChannel(); if (canUpdateVTX()) { handleVTXControlButton(); } #endif #ifdef USE_ACRO_TRAINER pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC)); #endif // USE_ACRO_TRAINER #ifdef USE_RC_SMOOTHING_FILTER if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) { beeper(BEEPER_RC_SMOOTHING_INIT_FAIL); } #endif pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); return true; }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef STM32F40_41xxx SetSysClock(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); ledInit(); #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif #if defined(USE_USART2) && defined(STM32F40_41xxx) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #if defined(USE_USART6) && defined(STM32F40_41xxx) pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); spiInit(SPI3); spiInit(SPI4); spiInit(SPI5); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else #if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(STM32F4DISCOVERY) i2cInit(I2C_DEVICE_INT); if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { #ifdef I2C_DEVICE_EXT i2cInit(I2C_DEVICE_EXT); #endif } #endif #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(3); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig); failsafeInit(&masterConfig.rxConfig); rxInit(&masterConfig.rxConfig); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(&masterConfig.batteryConfig); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { #ifdef COLIBRI if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) { ledStripEnable(); } #else ledStripEnable(); #endif } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #endif #if defined(SPRACINGF3) || defined(CC3D) || defined(COLIBRI) || defined(REVO) m25p16_init(); #endif flashfsInit(); #endif #ifdef BLACKBOX //initBlackbox(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } //gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet //timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif #include <stdio.h> #include "stm32f4xx_rcc.h" #include "stm32f4xx_gpio.h" GPIO_InitTypeDef GPIO_InitStruct; int main(void) { RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13 | GPIO_Pin_12; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOD, &GPIO_InitStruct); printf("Hello World!\r\n"); hello(); while (1) { static int count = 0; static int i; for (i = 0; i < 10000000; ++i) ; GPIO_ToggleBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); printf("%d\r\n", ++count); } //init(); /* while (1) { //loop(); int x = 1;//processLoopback(); }*/ }
void init(void) { printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; // initialize IO (needed for all IO operations) IOInitGlobal(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif i2cSetOverclock(masterConfig.i2c_overclock); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(500); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR if (feature(FEATURE_SONAR)) { const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); if (sonarHardware) { pwm_params.useSonar = true; pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; } } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef USE_SERVOS pwm_params.useServos = isServoOutputEnabled(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmInit(&pwm_params); mixerUsePWMIOConfiguration(); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .ioTag = IO_TAG(BEEPER), #ifdef BEEPER_INVERTED .isOD = false, .isInverted = true #else .isOD = true, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.isOD = false; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef GPS if (feature(FEATURE_GPS)) { gpsPreInit(&masterConfig.gpsConfig); } #endif // Set gyro sampling rate divider before initialization gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator); if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); } #endif #ifdef NAV navigationInit( &masterConfig.navConfig, ¤tProfile->pidProfile, ¤tProfile->rcControlsConfig, &masterConfig.rxConfig, &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig ); #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); /* Setup scheduler */ schedulerInit(); rescheduleTask(TASK_GYROPID, targetLooptime); setTaskEnabled(TASK_GYROPID, true); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif while (true) { scheduler(); processLoopback(); } }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; updateBattery(); } } if (feature(FEATURE_CURRENT_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } } beeperUpdate(); //call periodic beeper handler if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } #ifdef TELEMETRY telemetryCheckState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
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 }
void mwDisarm(void) { if (ARMING_FLAG(ARMED)) DISABLE_ARMING_FLAG(ARMED); }
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 init(void) { drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif i2cSetOverclock(masterConfig.i2c_highspeed); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, #ifdef BEEPER_INVERTED .gpioMode = Mode_Out_PP, .isInverted = true #else .gpioMode = Mode_Out_OD, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef BUTTONS buttonsInit(); if (!isMPUSoftReset()) { buttonsHandleColdBootButtonPresses(); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR const sonarHardware_t *sonarHardware = NULL; if (feature(FEATURE_SONAR)) { sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); sonarGPIOConfig_t sonarGPIOConfig = { .gpio = SONAR_GPIO, .triggerPin = sonarHardware->echo_pin, .echoPin = sonarHardware->trigger_pin, }; pwm_params.sonarGPIOConfig = &sonarGPIOConfig; } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2); #endif #if defined(USE_UART3) pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3); #endif #if defined(USE_UART4) pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4); #endif #if defined(USE_UART5) pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef SONAR pwm_params.useSonar = feature(FEATURE_SONAR); #endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params); mixerUsePWMIOConfiguration(pwmIOConfiguration); debug[2] = pwmIOConfiguration->pwmInputCount; debug[3] = pwmIOConfiguration->ppmInputCount; if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_UART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); #ifdef USE_SERVOS mixerInitialiseServoFiltering(targetLooptime); #endif #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); /* Setup scheduler */ if (masterConfig.gyroSync) { rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME); } else { rescheduleTask(TASK_GYROPID, targetLooptime); } setTaskEnabled(TASK_GYROPID, true); setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif while (1) { scheduler(); processLoopback(); } } void HardFault_Handler(void) { // fall out of the sky uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredStateForMotors) == requiredStateForMotors) { stopMotors(); } #ifdef TRANSPONDER // prevent IR LEDs from burning out. uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED; if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) { transponderIrDisable(); } #endif while (1); }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static batteryState_e batteryState = BATTERY_OK; static uint8_t vbatTimer = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(heading - headFreeModeHold); float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryState = calculateBatteryState(); } if (feature(FEATURE_CURRENT_METER)) { updateCurrentMeter(vbatCycleTime); } vbatCycleTime = 0; } } beepcodeUpdateState(batteryState); if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { LED0_TOGGLE; DISABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (ARMING_FLAG(OK_TO_ARM)) { disableWarningLed(); } else { enableWarningLed(currentTime); } updateWarningLed(currentTime); } #ifdef TELEMETRY checkTelemetryState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void failsafeUpdateState(void) { if (!failsafeIsMonitoring()) { return; } bool receivingRxData = failsafeIsReceivingRxData(); bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; if (!receivingRxData) { beeperMode = BEEPER_RX_LOST; } bool reprocessState; do { reprocessState = false; switch (failsafeState.phase) { case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData reprocessState = true; } else if (!receivingRxData) { if (millis() > failsafeState.throttleLowPeriod) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData } else { failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; } reprocessState = true; } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (failsafeSwitchIsOn) { ENABLE_FLIGHT_MODE(FAILSAFE_MODE); } else { DISABLE_FLIGHT_MODE(FAILSAFE_MODE); } // Throttle low period expired (= low long enough for JustDisarm) failsafeState.throttleLowPeriod = 0; } break; case FAILSAFE_RX_LOSS_DETECTED: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { // Stabilize, and set Throttle to specified level failsafeActivate(); } reprocessState = true; break; case FAILSAFE_LANDING: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } if (armed) { failsafeApplyControlInput(); beeperMode = BEEPER_RX_LOST_LANDING; } if (failsafeShouldHaveCausedLandingByNow() || !armed) { failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.phase = FAILSAFE_LANDED; reprocessState = true; } break; case FAILSAFE_LANDED: ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link mwDisarm(); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; reprocessState = true; break; case FAILSAFE_RX_LOSS_MONITORING: // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. if (receivingRxData) { if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { DISABLE_ARMING_FLAG(PREVENT_ARMING); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } } } else { failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; } break; case FAILSAFE_RX_LOSS_RECOVERED: // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // This is to prevent that JustDisarm is activated on the next iteration. // Because that would have the effect of shutting down failsafe handling on intermittent connections. failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); reprocessState = true; break; default: break; } } while (reprocessState); if (beeperMode != BEEPER_SILENCE) { beeper(beeperMode); } }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); detectHardwareRevision(); systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); ledInit(); 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; } } delay(100); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); memset(&pwm_params, 0, sizeof(pwm_params)); 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; } // 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; pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); 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); pwm_params.useSonar = feature(FEATURE_SONAR); pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; beeperConfig_t beeperConfig = { .gpioPeripheral = BEEP_PERIPHERAL, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioMode = Mode_Out_PP, .isInverted = true }; beeperInit(&beeperConfig); initInverter(); spiInit(SPI1); spiInit(SPI2); updateHardwareRevision(); serialRemovePort(SERIAL_PORT_USART3); i2cInit(I2C_DEVICE); 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 = true; // optional ADC5 input on rev.5 hardware adcInit(&adc_params); initBoardAlignment(&masterConfig.boardAlignment); if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; if (sensors(SENSOR_MAG)) compassInit(); imuInit(); mspInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig); failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig); if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } if (feature(FEATURE_SONAR)) { sonarInit(sonarHardware); } ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } m25p16_init(); flashfsInit(); initBlackbox(); previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); if (feature(FEATURE_DISPLAY)) { displayResetPageCycling(); displayEnablePageCycling(); } // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } int main(void) { init(); //Mine printf("\r\n"); printf("Init Finished!\r\n"); printf("System Init need %d ms\r\n", millis()); printf("############# Begin Test ###############\r\n"); printf("############# End Test ###############\r\n"); while (1) { loop(); } } void HardFault_Handler(void) { // fall out of the sky uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY; if ((systemState & requiredState) == requiredState) { stopMotors(); } while (1); }
void annexCode(void) { int32_t tmp; for (int axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupPitchRoll(tmp); } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupYaw(tmp) * -1; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; } } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); if (axis == ROLL || axis == PITCH) { if (rcControlsConfig()->deadband) { if (tmp > rcControlsConfig()->deadband) { tmp -= rcControlsConfig()->deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (rcControlsConfig()->yaw_deadband) { if (tmp > rcControlsConfig()->yaw_deadband) { tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } if (rcData[axis] < rxConfig()->midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); debug[3] = ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } debug[3] = ARMING_FLAG(OK_TO_ARM); warningLedUpdate(); } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); }