/* disk_initialize * * Set up the disk. */ DSTATUS disk_initialize (BYTE drv) { if (drv == 0 && sdcard_init() == 0) { Stat &= ~STA_NOINIT; } return Stat; }
void board_init(void) { printf("\n\n"); printf("BWCT FSB-A920-1\n"); printf("http://www.bwct.de\n"); printf("\n"); #if defined(SDRAM_128M) printf("AT92RM9200 180MHz 128MB\n"); #else printf("AT92RM9200 180MHz 64MB\n"); #endif printf("Initialising USART0\n"); USART0_Init(); printf("Initialising USART1\n"); USART1_Init(); printf("Initialising USART2\n"); USART2_Init(); printf("Initialising USART3\n"); USART3_Init(); printf("Initialising TWI\n"); EEInit(); printf("Initialising DS1672\n"); DS1672_Init(); printf("Initialising Ethernet\n"); printf("MAC %x:%x:%x:%x:%x:%x\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); EMAC_Init(); EMAC_SetMACAddress(mac); printf("Initialising SD-card\n"); sdcard_init(); }
void device_init(void) { i2c_init(); // I2C bus rtc_init(); // real time clock sdcard_init(); // SD card ieeehw_init(); // IEEE-488 hardware ieee_init(8); // hardware-independent part; registers as bus }
/******************************************************************************* * Function Name : Read_Memory * Description : Handle the Read operation from the microSD card. * Input : None. * Output : None. * Return : None. *******************************************************************************/ static int8_t STORAGE_Init (uint8_t lun) { UNUSED(lun); LED0_OFF; sdcard_init(sdcardConfig()); while (sdcard_poll() == 0); LED0_ON; return 0; }
void board_init(void) { InitEEPROM(); MacFromEE(); EMAC_Init(); EMAC_SetMACAddress(mac); while (sdcard_init() == 0) printf("Looking for SD card\n"); }
static esp_err_t _sdcard_init(esp_periph_handle_t self) { periph_sdcard_t *sdcard = esp_periph_get_data(self); esp_err_t ret = sdcard_init(sdcard->card_detect_pin, sdcard_gpio_intr_handler, self); if (sdcard_is_exist()) { ret |= periph_sdcard_mount(self); } else { ESP_LOGE(TAG, "no sdcard detect"); } esp_periph_start_timer(self, 1000/portTICK_RATE_MS, sdcard_timer_handler); return ESP_OK; }
void picture_transfer_send(unsigned char *filename) { unsigned char *ready_msg = "S"; unsigned char length_of_send_msg_length[2]; memset(length_of_send_msg_length, '\0', sizeof(length_of_send_msg_length)); unsigned char send_msg_length[10]; memset(send_msg_length, '\0', sizeof(send_msg_length)); char * imagedata = NULL; int imagelength; int offset; int result; sdcard_init(); result = bitmap_import_image(filename, &imagedata, &imagelength, &offset); if (result != 0) { printf("Error with importing image\n"); return; } //convert imagelength to string snprintf(send_msg_length, sizeof(send_msg_length), "%d", imagelength); //get length of send_msg_length snprintf(length_of_send_msg_length, sizeof(length_of_send_msg_length), "%d", strlen(send_msg_length)); //send ready message to start communication bluetooth_send_command(ready_msg); printf("Ready message sent!\n"); //send length of the string length of the final message picture_transfer_send_communication(length_of_send_msg_length, sizeof(length_of_send_msg_length), STRING_LENGTH); printf("Length of string length message sent!\n"); //send length of the final message picture_transfer_send_communication(send_msg_length, sizeof(send_msg_length), MESSAGE_LENGTH); printf("String length message sent!\n"); //send final message picture_transfer_send_communication(imagedata, imagelength, FINAL); printf("Final message sent!\n"); free(imagedata); }
DSTATUS disk_initialize(BYTE pdrv) { sink_sd_card_t* sink = sink_sd_card_from_pdrv(pdrv); int ret_code = sdcard_init(sink->sd_card_regs); int err_code = ret_code & ~SD_HCS_SET; //clear HCS flag in return value for error code if (ret_code & SD_HCS_SET) { sink->sdhc_card = 1; } if (err_code == SD_NO_ERROR) { sink->status = 0; } else { sink->status = STA_NOINIT; } sink->sd_error_code |= err_code; //sink->sdhc_card = 1; //TODO return sink->status; }
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 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); }
int main(void) { // TODO disable JTAG // Stack limit should be less than real stack size, so we have a chance // to recover from limit hit. (Limit is measured in bytes.) mp_stack_set_limit((char*)&_ram_end - (char*)&_heap_end - 1024); /* STM32F4xx HAL library initialization: - Configure the Flash prefetch, instruction and Data caches - Configure the Systick to generate an interrupt each 1 msec - Set NVIC Group Priority to 4 - Global MSP (MCU Support Package) initialization */ HAL_Init(); // set the system clock to be HSE SystemClock_Config(); // enable GPIO clocks __GPIOA_CLK_ENABLE(); __GPIOB_CLK_ENABLE(); __GPIOC_CLK_ENABLE(); __GPIOD_CLK_ENABLE(); // enable the CCM RAM __CCMDATARAMEN_CLK_ENABLE(); #if 0 #if defined(NETDUINO_PLUS_2) { GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; #if MICROPY_HW_HAS_SDCARD // Turn on the power enable for the sdcard (PB1) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET); #endif // Turn on the power for the 5V on the expansion header (PB2) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET); } #endif #endif // basic sub-system init pendsv_init(); timer_tim3_init(); led_init(); #if MICROPY_HW_HAS_SWITCH switch_init0(); #endif int first_soft_reset = true; soft_reset: // check if user switch held to select the reset mode led_state(1, 0); led_state(2, 1); led_state(3, 0); led_state(4, 0); uint reset_mode = 1; #if MICROPY_HW_HAS_SWITCH if (switch_get()) { for (uint i = 0; i < 3000; i++) { if (!switch_get()) { break; } HAL_Delay(20); if (i % 30 == 29) { if (++reset_mode > 3) { reset_mode = 1; } led_state(2, reset_mode & 1); led_state(3, reset_mode & 2); led_state(4, reset_mode & 4); } } // flash the selected reset mode for (uint i = 0; i < 6; i++) { led_state(2, 0); led_state(3, 0); led_state(4, 0); HAL_Delay(50); led_state(2, reset_mode & 1); led_state(3, reset_mode & 2); led_state(4, reset_mode & 4); HAL_Delay(50); } HAL_Delay(400); } #endif #if MICROPY_HW_ENABLE_RTC if (first_soft_reset) { rtc_init(); } #endif // more sub-system init #if MICROPY_HW_HAS_SDCARD if (first_soft_reset) { sdcard_init(); } #endif if (first_soft_reset) { storage_init(); } // GC init gc_init(&_heap_start, &_heap_end); // Micro Python init mp_init(); mp_obj_list_init(mp_sys_path, 0); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_)); // current dir (or base dir of the script) mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_flash)); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_flash_slash_lib)); mp_obj_list_init(mp_sys_argv, 0); // Change #if 0 to #if 1 if you want REPL on UART_6 (or another uart) // as well as on USB VCP #if 0 { mp_obj_t args[2] = { MP_OBJ_NEW_SMALL_INT(PYB_UART_6), MP_OBJ_NEW_SMALL_INT(115200), }; pyb_stdio_uart = pyb_uart_type.make_new((mp_obj_t)&pyb_uart_type, MP_ARRAY_SIZE(args), 0, args); } #else pyb_stdio_uart = NULL; #endif // Initialise low-level sub-systems. Here we need to very basic things like // zeroing out memory and resetting any of the sub-systems. Following this // we can run Python scripts (eg boot.py), but anything that is configurable // by boot.py must be set after boot.py is run. readline_init0(); pin_init0(); extint_init0(); timer_init0(); uart_init0(); #if MICROPY_HW_ENABLE_RNG rng_init0(); #endif i2c_init0(); spi_init0(); pyb_usb_init0(); // Initialise the local flash filesystem. // Create it if needed, and mount in on /flash. { // try to mount the flash FRESULT res = f_mount(&fatfs0, "/flash", 1); if (reset_mode == 3 || res == FR_NO_FILESYSTEM) { // no filesystem, or asked to reset it, so create a fresh one // LED on to indicate creation of LFS led_state(PYB_LED_R2, 1); uint32_t start_tick = HAL_GetTick(); res = f_mkfs("/flash", 0, 0); if (res == FR_OK) { // success creating fresh LFS } else { __fatal_error("could not create LFS"); } // set label f_setlabel("/flash/pybflash"); // create empty main.py FIL fp; f_open(&fp, "/flash/main.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // create .inf driver file f_open(&fp, "/flash/pybcdc.inf", FA_WRITE | FA_CREATE_ALWAYS); f_write(&fp, fresh_pybcdc_inf, sizeof(fresh_pybcdc_inf) - 1 /* don't count null terminator */, &n); f_close(&fp); // create readme file f_open(&fp, "/flash/README.txt", FA_WRITE | FA_CREATE_ALWAYS); f_write(&fp, fresh_readme_txt, sizeof(fresh_readme_txt) - 1 /* don't count null terminator */, &n); f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(start_tick, 200); led_state(PYB_LED_R2, 0); } else if (res == FR_OK) { // mount sucessful } else { __fatal_error("could not access LFS"); } } // The current directory is used as the boot up directory. // It is set to the internal flash filesystem by default. f_chdrive("/flash"); // Make sure we have a /flash/boot.py. Create it if needed. { FILINFO fno; #if _USE_LFN fno.lfname = NULL; fno.lfsize = 0; #endif FRESULT res = f_stat("/flash/boot.py", &fno); if (res == FR_OK) { if (fno.fattrib & AM_DIR) { // exists as a directory // TODO handle this case // see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation } else { // exists as a file, good! } } else { // doesn't exist, create fresh file // LED on to indicate creation of boot.py led_state(PYB_LED_R2, 1); uint32_t start_tick = HAL_GetTick(); FIL fp; f_open(&fp, "/flash/boot.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(start_tick, 200); led_state(PYB_LED_R2, 0); } } #if defined(USE_DEVICE_MODE) usb_storage_medium_t usb_medium = USB_STORAGE_MEDIUM_FLASH; #endif #if MICROPY_HW_HAS_SDCARD // if an SD card is present then mount it on /sd/ if (sdcard_is_present()) { FRESULT res = f_mount(&fatfs1, "/sd", 1); if (res != FR_OK) { printf("[SD] could not mount SD card\n"); } else { // use SD card as current directory f_chdrive("/sd"); // TODO these should go before the /flash entries in the path mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_sd)); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_sd_slash_lib)); if (first_soft_reset) { // use SD card as medium for the USB MSD #if defined(USE_DEVICE_MODE) usb_medium = USB_STORAGE_MEDIUM_SDCARD; #endif } } } #endif // reset config variables; they should be set by boot.py pyb_config_main = MP_OBJ_NULL; pyb_config_usb_mode = MP_OBJ_NULL; // run boot.py, if it exists // TODO perhaps have pyb.reboot([bootpy]) function to soft-reboot and execute custom boot.py if (reset_mode == 1) { const char *boot_py = "boot.py"; FRESULT res = f_stat(boot_py, NULL); if (res == FR_OK) { int ret = pyexec_file(boot_py); if (ret & PYEXEC_FORCED_EXIT) { goto soft_reset_exit; } if (!ret) { flash_error(4); } } } // turn boot-up LEDs off led_state(2, 0); led_state(3, 0); led_state(4, 0); // Now we initialise sub-systems that need configuration from boot.py, // or whose initialisation can be safely deferred until after running // boot.py. #if defined(USE_HOST_MODE) // USB host pyb_usb_host_init(); #elif defined(USE_DEVICE_MODE) // USB device usb_device_mode_t usb_mode = USB_DEVICE_MODE_CDC_MSC; // if we are not in reset_mode==1, this config variable will always be NULL if (pyb_config_usb_mode != MP_OBJ_NULL) { if (strcmp(mp_obj_str_get_str(pyb_config_usb_mode), "CDC+HID") == 0) { usb_mode = USB_DEVICE_MODE_CDC_HID; } } pyb_usb_dev_init(usb_mode, usb_medium); #endif #if MICROPY_HW_HAS_MMA7660 // MMA accel: init and reset accel_init(); #endif #if MICROPY_HW_ENABLE_SERVO // servo servo_init(); #endif #if MICROPY_HW_ENABLE_DAC // DAC dac_init(); #endif mod_network_init(); // At this point everything is fully configured and initialised. // Run the main script from the current directory. if (reset_mode == 1 && pyexec_mode_kind == PYEXEC_MODE_FRIENDLY_REPL) { const char *main_py; if (pyb_config_main == MP_OBJ_NULL) { main_py = "main.py"; } else { main_py = mp_obj_str_get_str(pyb_config_main); } FRESULT res = f_stat(main_py, NULL); if (res == FR_OK) { int ret = pyexec_file(main_py); if (ret & PYEXEC_FORCED_EXIT) { goto soft_reset_exit; } if (!ret) { flash_error(3); } } } // Main script is finished, so now go into REPL mode. // The REPL mode can change, or it can request a soft reset. for (;;) { if (pyexec_mode_kind == PYEXEC_MODE_RAW_REPL) { if (pyexec_raw_repl() != 0) { break; } } else { if (pyexec_friendly_repl() != 0) { break; } } } soft_reset_exit: // soft reset printf("PYB: sync filesystems\n"); storage_flush(); printf("PYB: soft reboot\n"); timer_deinit(); uart_deinit(); first_soft_reset = false; goto soft_reset; }
int main(void) { // TODO disable JTAG /* STM32F4xx HAL library initialization: - Configure the Flash prefetch, instruction and Data caches - Configure the Systick to generate an interrupt each 1 msec - Set NVIC Group Priority to 4 - Global MSP (MCU Support Package) initialization */ HAL_Init(); // set the system clock to be HSE SystemClock_Config(); // enable GPIO clocks __GPIOA_CLK_ENABLE(); __GPIOB_CLK_ENABLE(); __GPIOC_CLK_ENABLE(); __GPIOD_CLK_ENABLE(); // enable the CCM RAM __CCMDATARAMEN_CLK_ENABLE(); #if 0 #if defined(NETDUINO_PLUS_2) { GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; #if MICROPY_HW_HAS_SDCARD // Turn on the power enable for the sdcard (PB1) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET); #endif // Turn on the power for the 5V on the expansion header (PB2) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET); } #endif #endif // basic sub-system init pendsv_init(); timer_tim3_init(); led_init(); switch_init0(); int first_soft_reset = true; soft_reset: // check if user switch held to select the reset mode led_state(1, 0); led_state(2, 1); led_state(3, 0); led_state(4, 0); uint reset_mode = 1; #if MICROPY_HW_HAS_SWITCH if (switch_get()) { for (uint i = 0; i < 3000; i++) { if (!switch_get()) { break; } HAL_Delay(20); if (i % 30 == 29) { if (++reset_mode > 3) { reset_mode = 1; } led_state(2, reset_mode & 1); led_state(3, reset_mode & 2); led_state(4, reset_mode & 4); } } // flash the selected reset mode for (uint i = 0; i < 6; i++) { led_state(2, 0); led_state(3, 0); led_state(4, 0); HAL_Delay(50); led_state(2, reset_mode & 1); led_state(3, reset_mode & 2); led_state(4, reset_mode & 4); HAL_Delay(50); } HAL_Delay(400); } #endif #if MICROPY_HW_ENABLE_RTC if (first_soft_reset) { rtc_init(); } #endif // more sub-system init #if MICROPY_HW_HAS_SDCARD if (first_soft_reset) { sdcard_init(); } #endif if (first_soft_reset) { storage_init(); } // GC init gc_init(&_heap_start, &_heap_end); // Change #if 0 to #if 1 if you want REPL on USART_6 (or another usart) // as well as on USB VCP #if 0 pyb_usart_global_debug = pyb_Usart(MP_OBJ_NEW_SMALL_INT(PYB_USART_YA), MP_OBJ_NEW_SMALL_INT(115200)); #else pyb_usart_global_debug = NULL; #endif // Micro Python init qstr_init(); mp_init(); mp_obj_list_init(mp_sys_path, 0); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_)); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_lib)); mp_obj_list_init(mp_sys_argv, 0); readline_init(); exti_init(); #if MICROPY_HW_HAS_SWITCH // must come after exti_init switch_init(); #endif #if MICROPY_HW_HAS_LCD // LCD init (just creates class, init hardware by calling LCD()) lcd_init(); #endif pin_map_init(); // local filesystem init { // try to mount the flash FRESULT res = f_mount(&fatfs0, "0:", 1); if (reset_mode == 3 || res == FR_NO_FILESYSTEM) { // no filesystem, or asked to reset it, so create a fresh one // LED on to indicate creation of LFS led_state(PYB_LED_R2, 1); uint32_t start_tick = HAL_GetTick(); res = f_mkfs("0:", 0, 0); if (res == FR_OK) { // success creating fresh LFS } else { __fatal_error("could not create LFS"); } // create empty main.py FIL fp; f_open(&fp, "0:/main.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // create .inf driver file f_open(&fp, "0:/pybcdc.inf", FA_WRITE | FA_CREATE_ALWAYS); f_write(&fp, fresh_pybcdc_inf, sizeof(fresh_pybcdc_inf) - 1 /* don't count null terminator */, &n); f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(start_tick, 200); led_state(PYB_LED_R2, 0); } else if (res == FR_OK) { // mount sucessful } else { __fatal_error("could not access LFS"); } } // make sure we have a 0:/boot.py { FILINFO fno; #if _USE_LFN fno.lfname = NULL; fno.lfsize = 0; #endif FRESULT res = f_stat("0:/boot.py", &fno); if (res == FR_OK) { if (fno.fattrib & AM_DIR) { // exists as a directory // TODO handle this case // see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation } else { // exists as a file, good! } } else { // doesn't exist, create fresh file // LED on to indicate creation of boot.py led_state(PYB_LED_R2, 1); uint32_t start_tick = HAL_GetTick(); FIL fp; f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(start_tick, 200); led_state(PYB_LED_R2, 0); } } // root device defaults to internal flash filesystem uint root_device = 0; #if defined(USE_DEVICE_MODE) usb_storage_medium_t usb_medium = USB_STORAGE_MEDIUM_FLASH; #endif #if MICROPY_HW_HAS_SDCARD // if an SD card is present then mount it on 1:/ if (reset_mode == 1 && sdcard_is_present()) { FRESULT res = f_mount(&fatfs1, "1:", 1); if (res != FR_OK) { printf("[SD] could not mount SD card\n"); } else { // use SD card as root device root_device = 1; if (first_soft_reset) { // use SD card as medium for the USB MSD #if defined(USE_DEVICE_MODE) usb_medium = USB_STORAGE_MEDIUM_SDCARD; #endif } } } #else // Get rid of compiler warning if no SDCARD is configured. (void)first_soft_reset; #endif // run <root>:/boot.py, if it exists if (reset_mode == 1) { const char *boot_file; if (root_device == 0) { boot_file = "0:/boot.py"; } else { boot_file = "1:/boot.py"; } FRESULT res = f_stat(boot_file, NULL); if (res == FR_OK) { if (!pyexec_file(boot_file)) { flash_error(4); } } } // turn boot-up LEDs off led_state(2, 0); led_state(3, 0); led_state(4, 0); #if defined(USE_HOST_MODE) // USB host pyb_usb_host_init(); #elif defined(USE_DEVICE_MODE) // USB device if (reset_mode == 1) { usb_device_mode_t usb_mode = USB_DEVICE_MODE_CDC_MSC; if (pyb_config_usb_mode != MP_OBJ_NULL) { if (strcmp(mp_obj_str_get_str(pyb_config_usb_mode), "CDC+HID") == 0) { usb_mode = USB_DEVICE_MODE_CDC_HID; } } pyb_usb_dev_init(usb_mode, usb_medium); } else { pyb_usb_dev_init(USB_DEVICE_MODE_CDC_MSC, usb_medium); } #endif #if MICROPY_HW_ENABLE_RNG // RNG rng_init(); #endif #if MICROPY_HW_ENABLE_TIMER // timer //timer_init(); #endif // I2C i2c_init(); #if MICROPY_HW_HAS_MMA7660 // MMA accel: init and reset accel_init(); #endif #if MICROPY_HW_ENABLE_SERVO // servo servo_init(); #endif #if MICROPY_HW_ENABLE_DAC // DAC dac_init(); #endif // now that everything is initialised, run main script if (reset_mode == 1 && pyexec_mode_kind == PYEXEC_MODE_FRIENDLY_REPL) { vstr_t *vstr = vstr_new(); vstr_printf(vstr, "%d:/", root_device); if (pyb_config_main == MP_OBJ_NULL) { vstr_add_str(vstr, "main.py"); } else { vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main)); } FRESULT res = f_stat(vstr_str(vstr), NULL); if (res == FR_OK) { if (!pyexec_file(vstr_str(vstr))) { flash_error(3); } } vstr_free(vstr); } #if 0 #if MICROPY_HW_HAS_WLAN // wifi pyb_wlan_init(); pyb_wlan_start(); #endif #endif // enter REPL // REPL mode can change, or it can request a soft reset for (;;) { if (pyexec_mode_kind == PYEXEC_MODE_RAW_REPL) { if (pyexec_raw_repl() != 0) { break; } } else { if (pyexec_friendly_repl() != 0) { break; } } } printf("PYB: sync filesystems\n"); storage_flush(); printf("PYB: soft reboot\n"); first_soft_reset = false; goto soft_reset; }
int main(void) { cli(); /* We need to use a timer to control how long our main loop iterations are. * Experiments showed that the main loop was executing approximately every * 3.2 milliseconds, which is too quick for the sdcard to write a block * of data. Here we are configuring Timer1 because it is a "16-bit" timer * which would allow us to count to values greater than (2^8)-1. For a pre- * scale value of 64, the timer will count up to 16,000,000/64 = 250,000. * This gives us a timer period of 250,000 ticks/sec = 4 microseconds/tick. * Suppose we want the main loop to run 40 times per second (40 Hz), we would * need to restart the loop when Timer1 reaches the value: 250,000/40 = 6250 * See Atmel datasheet for Mega, Section 17.11 */ TCCR1A = 0b00000000; // Normal operation; no waveform generation by default TCCR1B = 0b00000011; // No input capture, waveform gen; prescaler = 64 TCCR1C = 0b00000000; // No output compare TIMSK1 = 0b00000000; // TIMING DEBUG - Digital Pin 4 DDRG |= (1 << 5); char msg[64]; if (button_init() && uwrite_init() && cmps10_init() && sdcard_init()) { uwrite_print_buff("All systems go!\r\n"); return 0; } else { uwrite_print_buff("There was an error during init\r\n"); return 1; } memset(msg, 0, sizeof(msg)); memset(&statevars, 0, sizeof(statevars)); statevars.prefix = 0xDADAFEED; statevars.suffix = 0xCAFEBABE; uint32_t iterations = 0; sei(); TIMSK1 = 0b00000001; while (1) { // TIMING DEBUG FOR OSCILLOSCOPE PORTG |= (1 << 5); TCNT1 = 0; button_update(); cmps10_update_all(); statevars.main_loop_counter = iterations; mainloop_timer_overflow = 0; // TIMING DEBUG FOR OSCILLOSCOPE PORTG &= (0 << 5); if (button_is_pressed()) { uwrite_print_buff("The button is pressed! The LED should be on.\r\n"); led_turn_on(); statevars.mission_started = 1; } else { led_turn_off(); statevars.mission_started = 0; } snprintf(msg, sizeof(msg), "Heading: %u Pitch: %d Roll: %hhd\r\n", cmps10_heading, cmps10_pitch, cmps10_roll); // Needed to cast these values to display negative pitch and roll values //(int8_t) statevars.heading_raw, (int8_t) statevars.pitch_deg, (int8_t) statevars.roll_deg); uwrite_print_buff(msg); sdcard_write_data(); iterations++; if (iterations > 256) { uwrite_print_buff("Finished collecting data!\r\n"); break; } /* Ensure that the main loop period is as long as we want it to be. * This means (1) triggering the main loop to restart we notice it is * running too long, and (2) performing busy waiting if the instructions * above finish before the desired loop duration. */ while (1) { if (mainloop_timer_overflow) { break; } if (TCNT1 >= MAINLOOP_PERIOD_TICKS) { break; } } } return 0; }
void init(void) { printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; // initialize IO (needed for all IO operations) IOInitGlobal(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif i2cSetOverclock(masterConfig.i2c_overclock); #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif systemInit(); // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(500); timerInit(); // timer must be initialized before any channel is allocated serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR if (feature(FEATURE_SONAR)) { const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); if (sonarHardware) { pwm_params.useSonar = true; pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; } } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef USE_SERVOS pwm_params.useServos = isServoOutputEnabled(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmInit(&pwm_params); mixerUsePWMIOConfiguration(); if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .ioTag = IO_TAG(BEEPER), #ifdef BEEPER_INVERTED .isOD = false, .isInverted = true #else .isOD = true, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.isOD = false; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef GPS if (feature(FEATURE_GPS)) { gpsPreInit(&masterConfig.gpsConfig); } #endif // Set gyro sampling rate divider before initialization gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator); if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); } #endif #ifdef NAV navigationInit( &masterConfig.navConfig, ¤tProfile->pidProfile, ¤tProfile->rcControlsConfig, &masterConfig.rxConfig, &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig ); #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(); } #elif defined(USE_FLASH_M25P16) m25p16_init(); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif #ifdef BLACKBOX initBlackbox(); #endif gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); /* Setup scheduler */ schedulerInit(); rescheduleTask(TASK_GYROPID, targetLooptime); setTaskEnabled(TASK_GYROPID, true); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif while (true) { scheduler(); processLoopback(); } }
int main(void) { // Stack limit should be less than real stack size, so we // had chance to recover from limit hit. mp_stack_set_limit((char*)&_ram_end - (char*)&_heap_end - 1024); /* STM32F4xx HAL library initialization: - Configure the Flash prefetch, instruction and Data caches - Configure the Systick to generate an interrupt each 1 msec - Set NVIC Group Priority to 4 - Global MSP (MCU Support Package) initialization */ HAL_Init(); // basic sub-system init pendsv_init(); timer_tim3_init(); led_init(); soft_reset: // check if user switch held to select the reset mode led_state(LED_RED, 1); led_state(LED_GREEN, 1); led_state(LED_BLUE, 1); #if MICROPY_HW_ENABLE_RTC rtc_init(); #endif // GC init gc_init(&_heap_start, &_heap_end); // Micro Python init mp_init(); mp_obj_list_init(mp_sys_path, 0); mp_obj_list_init(mp_sys_argv, 0); readline_init0(); pin_init0(); extint_init0(); timer_init0(); rng_init0(); i2c_init0(); spi_init0(); uart_init0(); pyb_usb_init0(); usbdbg_init(); if (sensor_init() != 0) { __fatal_error("Failed to init sensor"); } /* Export functions to the global python namespace */ mp_store_global(qstr_from_str("randint"), (mp_obj_t)&py_randint_obj); mp_store_global(qstr_from_str("cpu_freq"), (mp_obj_t)&py_cpu_freq_obj); mp_store_global(qstr_from_str("Image"), (mp_obj_t)&py_image_load_image_obj); mp_store_global(qstr_from_str("HaarCascade"), (mp_obj_t)&py_image_load_cascade_obj); mp_store_global(qstr_from_str("FreakDesc"), (mp_obj_t)&py_image_load_descriptor_obj); mp_store_global(qstr_from_str("FreakDescSave"), (mp_obj_t)&py_image_save_descriptor_obj); mp_store_global(qstr_from_str("LBPDesc"), (mp_obj_t)&py_image_load_lbp_obj); mp_store_global(qstr_from_str("vcp_is_connected"), (mp_obj_t)&py_vcp_is_connected_obj); if (sdcard_is_present()) { sdcard_init(); FRESULT res = f_mount(&fatfs, "1:", 1); if (res != FR_OK) { __fatal_error("could not mount SD\n"); } // Set CWD and USB medium to SD f_chdrive("1:"); pyb_usb_storage_medium = PYB_USB_STORAGE_MEDIUM_SDCARD; } else { storage_init(); // try to mount the flash FRESULT res = f_mount(&fatfs, "0:", 1); if (res == FR_NO_FILESYSTEM) { // create a fresh fs make_flash_fs(); } else if (res != FR_OK) { __fatal_error("could not access LFS\n"); } // Set CWD and USB medium to flash f_chdrive("0:"); pyb_usb_storage_medium = PYB_USB_STORAGE_MEDIUM_FLASH; } // turn boot-up LEDs off led_state(LED_RED, 0); led_state(LED_GREEN, 0); led_state(LED_BLUE, 0); // init USB device to default setting if it was not already configured if (!(pyb_usb_flags & PYB_USB_FLAG_USB_MODE_CALLED)) { pyb_usb_dev_init(USBD_VID, USBD_PID_CDC_MSC, USBD_MODE_CDC_MSC, NULL); } // Run the main script from the current directory. FRESULT res = f_stat("main.py", NULL); if (res == FR_OK) { if (!pyexec_file("main.py")) { nlr_buf_t nlr; if (nlr_push(&nlr) == 0) { flash_error(3); nlr_pop(); } } } // Enter REPL nlr_buf_t nlr; for (;;) { if (nlr_push(&nlr) == 0) { while (usbdbg_script_ready()) { nlr_buf_t nlr; vstr_t *script_buf = usbdbg_get_script(); // clear script flag usbdbg_clr_script(); // execute the script if (nlr_push(&nlr) == 0) { pyexec_push_scope(); // parse and compile script mp_lexer_t *lex = mp_lexer_new_from_str_len(MP_QSTR__lt_stdin_gt_, vstr_str(script_buf), vstr_len(script_buf), 0); mp_parse_node_t pn = mp_parse(lex, MP_PARSE_FILE_INPUT); mp_obj_t script = mp_compile(pn, lex->source_name, MP_EMIT_OPT_NONE, false); // execute the script mp_call_function_0(script); nlr_pop(); } else { mp_obj_print_exception(&mp_plat_print, (mp_obj_t)nlr.ret_val); } pyexec_pop_scope(); } // clear script flag usbdbg_clr_script(); // no script run REPL pyexec_friendly_repl(); nlr_pop(); } } printf("PYB: sync filesystems\n"); storage_flush(); printf("PYB: soft reboot\n"); goto soft_reset; }
int main(void) { FRESULT f_res; int sensor_init_ret; // Stack limit should be less than real stack size, so we // had chance to recover from limit hit. mp_stack_set_limit((char*)&_ram_end - (char*)&_heap_end - 1024); /* STM32F4xx HAL library initialization: - Configure the Flash prefetch, instruction and Data caches - Configure the Systick to generate an interrupt each 1 msec - Set NVIC Group Priority to 4 - Global MSP (MCU Support Package) initialization */ HAL_Init(); // basic sub-system init pendsv_init(); timer_tim3_init(); led_init(); soft_reset: // check if user switch held to select the reset mode led_state(LED_RED, 1); led_state(LED_GREEN, 1); led_state(LED_BLUE, 1); #if MICROPY_HW_ENABLE_RTC rtc_init(); #endif // GC init gc_init(&_heap_start, &_heap_end); // Micro Python init mp_init(); mp_obj_list_init(mp_sys_path, 0); mp_obj_list_init(mp_sys_argv, 0); readline_init0(); pin_init0(); extint_init0(); timer_init0(); rng_init0(); i2c_init0(); spi_init0(); uart_init0(); pyb_usb_init0(); usbdbg_init(); sensor_init_ret = sensor_init(); /* Export functions to the global python namespace */ mp_store_global(qstr_from_str("randint"), (mp_obj_t)&py_randint_obj); mp_store_global(qstr_from_str("cpu_freq"), (mp_obj_t)&py_cpu_freq_obj); mp_store_global(qstr_from_str("vcp_is_connected"), (mp_obj_t)&py_vcp_is_connected_obj); if (sdcard_is_present()) { sdcard_init(); FRESULT res = f_mount(&fatfs, "1:", 1); if (res != FR_OK) { __fatal_error("could not mount SD\n"); } // Set CWD and USB medium to SD f_chdrive("1:"); pyb_usb_storage_medium = PYB_USB_STORAGE_MEDIUM_SDCARD; } else { storage_init(); // try to mount the flash FRESULT res = f_mount(&fatfs, "0:", 1); if (res == FR_NO_FILESYSTEM) { // create a fresh fs make_flash_fs(); } else if (res != FR_OK) { __fatal_error("could not access LFS\n"); } // Set CWD and USB medium to flash f_chdrive("0:"); pyb_usb_storage_medium = PYB_USB_STORAGE_MEDIUM_FLASH; } // turn boot-up LEDs off led_state(LED_RED, 0); led_state(LED_GREEN, 0); led_state(LED_BLUE, 0); // init USB device to default setting if it was not already configured if (!(pyb_usb_flags & PYB_USB_FLAG_USB_MODE_CALLED)) { pyb_usb_dev_init(USBD_VID, USBD_PID_CDC_MSC, USBD_MODE_CDC_MSC, NULL); } // check sensor init result if (sensor_init_ret != 0) { char buf[512]; snprintf(buf, sizeof(buf), "Failed to init sensor, error:%d", sensor_init_ret); __fatal_error(buf); } // Run self tests the first time only f_res = f_stat("selftest.py", NULL); if (f_res == FR_OK) { nlr_buf_t nlr; if (nlr_push(&nlr) == 0) { // Parse, compile and execute the self-tests script. pyexec_file("selftest.py"); nlr_pop(); } else { // Get the exception message. TODO: might be a hack. mp_obj_str_t *str = mp_obj_exception_get_value((mp_obj_t)nlr.ret_val); // If any of the self-tests fail log the exception message // and loop forever. Note: IDE exceptions will not be caught. __fatal_error((const char*) str->data); } // Success: remove self tests script and flush cache f_unlink("selftest.py"); storage_flush(); } // Run the main script from the current directory. f_res = f_stat("main.py", NULL); if (f_res == FR_OK) { nlr_buf_t nlr; if (nlr_push(&nlr) == 0) { // Parse, compile and execute the main script. pyexec_file("main.py"); nlr_pop(); } else { mp_obj_print_exception(&mp_plat_print, (mp_obj_t)nlr.ret_val); if (nlr_push(&nlr) == 0) { flash_error(3); nlr_pop(); }// if this gets interrupted again ignore it. } } // Enter REPL nlr_buf_t nlr; for (;;) { if (nlr_push(&nlr) == 0) { while (usbdbg_script_ready()) { nlr_buf_t nlr; vstr_t *script_buf = usbdbg_get_script(); // clear debugging flags usbdbg_clear_flags(); // re-init MP mp_uint_t atomic_state = MICROPY_BEGIN_ATOMIC_SECTION(); mp_init(); MICROPY_END_ATOMIC_SECTION(atomic_state); // execute the script if (nlr_push(&nlr) == 0) { // parse, compile and execute script pyexec_str(script_buf); nlr_pop(); } else { mp_obj_print_exception(&mp_plat_print, (mp_obj_t)nlr.ret_val); } } // clear debugging flags usbdbg_clear_flags(); // re-init MP mp_uint_t atomic_state = MICROPY_BEGIN_ATOMIC_SECTION(); mp_init(); MICROPY_END_ATOMIC_SECTION(atomic_state); // no script run REPL pyexec_friendly_repl(); nlr_pop(); } } printf("PYB: sync filesystems\n"); storage_flush(); printf("PYB: soft reboot\n"); goto soft_reset; }
int main(void) { // copy vector table to SRAM1! #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wnonnull" memcpy((char *)0x20000000, (const char)0x00000000, 0x200); #pragma GCC diagnostic pop // remap SRAM1 to 0x00000000 SYSCFG->MEMRMP |= 0x03; halInit(); chSysInit(); sdcard_init(); sysmon_init(); #if ENABLE_SERIAL_DEBUG // SD2 for serial debug output palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7) | PAL_MODE_INPUT); // RX palSetPadMode(GPIOA, 2, PAL_MODE_OUTPUT_PUSHPULL); // TX palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7)); // TX // 115200 baud static const SerialConfig sd2Cfg = {115200, 0, 0, 0}; sdStart(&SD2, &sd2Cfg); chprintf((BaseSequentialStream * )&SD2,"Hello world!\r\n"); #endif exception_init(); InitPatch0(); InitPConnection(); InitPWM(); // display SPI CS? palSetPadMode(GPIOC, 1, PAL_MODE_OUTPUT_PUSHPULL); palSetPad(GPIOC, 1); chThdSleepMilliseconds(10); palSetPadMode(SW2_PORT, SW2_PIN, PAL_MODE_INPUT_PULLDOWN); axoloti_board_init(); codec_init(); if (!palReadPad(SW2_PORT, SW2_PIN)) { // button S2 not pressed // watchdog_init(); chThdSleepMilliseconds(1); } start_dsp_thread(); adc_init(); axoloti_math_init(); midi_init(); #if ((BOARD_AXOLOTI_V03)||(BOARD_AXOLOTI_V05)) axoloti_control_init(); #endif ui_init(); StartLoadPatchTread(); #if (BOARD_AXOLOTI_V05) configSDRAM(); //memTest(); #endif #ifdef ENABLE_USB_HOST MY_USBH_Init(); #endif if (!exception_check()) { // only try booting a patch when no exception is to be reported #if ((BOARD_AXOLOTI_V03)||(BOARD_AXOLOTI_V05)) sdcard_attemptMountIfUnmounted(); if (fs_ready && !palReadPad(SW2_PORT, SW2_PIN)){ // button S2 not pressed FRESULT res; // res = f_stat("/start.bin", NULL); // if (res == FR_OK) { LoadPatch("/start.bin"); // } } #endif // if no patch booting or running yet // try loading from flash if (patchStatus) { // patch in flash sector 11 memcpy((uint8_t *)PATCHMAINLOC, (uint8_t *)PATCHFLASHLOC, PATCHFLASHSIZE); if ((*(uint32_t *)PATCHMAINLOC != 0xFFFFFFFF) && (*(uint32_t *)PATCHMAINLOC != 0)) { if (!palReadPad(SW2_PORT, SW2_PIN)) // button S2 not pressed StartPatch(); } } } while (1) { chThdSleepMilliseconds(1000); } }
int main(void) { // TODO disable JTAG // update the SystemCoreClock variable SystemCoreClockUpdate(); // set interrupt priority config to use all 4 bits for pre-empting NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); // enable the CCM RAM and the GPIO's RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN | RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN; #if MICROPY_HW_HAS_SDCARD { // configure SDIO pins to be high to start with (apparently makes it more robust) // FIXME this is not making them high, it just makes them outputs... GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // Configure PD.02 CMD line GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; GPIO_Init(GPIOD, &GPIO_InitStructure); } #endif #if defined(NETDUINO_PLUS_2) { GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; #if MICROPY_HW_HAS_SDCARD // Turn on the power enable for the sdcard (PB1) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET); #endif // Turn on the power for the 5V on the expansion header (PB2) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET); } #endif // basic sub-system init sys_tick_init(); pendsv_init(); led_init(); #if MICROPY_HW_ENABLE_RTC rtc_init(); #endif // turn on LED to indicate bootup led_state(PYB_LED_G1, 1); // more sub-system init #if MICROPY_HW_HAS_SDCARD sdcard_init(); #endif storage_init(); // uncomment these 2 lines if you want REPL on USART_6 (or another usart) as well as on USB VCP //pyb_usart_global_debug = PYB_USART_YA; //usart_init(pyb_usart_global_debug, 115200); int first_soft_reset = true; soft_reset: // GC init gc_init(&_heap_start, &_heap_end); // Micro Python init qstr_init(); mp_init(); mp_obj_list_init(mp_sys_path, 0); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_)); mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_lib)); mp_obj_list_init(mp_sys_argv, 0); exti_init(); #if MICROPY_HW_HAS_SWITCH switch_init(); #endif #if MICROPY_HW_HAS_LCD // LCD init (just creates class, init hardware by calling LCD()) lcd_init(); #endif #if MICROPY_HW_ENABLE_SERVO // servo servo_init(); #endif #if MICROPY_HW_ENABLE_TIMER // timer timer_init(); #endif #if MICROPY_HW_ENABLE_RNG // RNG RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_RNG, ENABLE); RNG_Cmd(ENABLE); #endif pin_map_init(); // add some functions to the builtin Python namespace mp_store_name(MP_QSTR_help, mp_make_function_n(0, pyb_help)); mp_store_name(MP_QSTR_open, mp_make_function_n(2, pyb_io_open)); // load the pyb module mp_module_register(MP_QSTR_pyb, (mp_obj_t)&pyb_module); // check if user switch held (initiates reset of filesystem) bool reset_filesystem = false; #if MICROPY_HW_HAS_SWITCH if (switch_get()) { reset_filesystem = true; for (int i = 0; i < 50; i++) { if (!switch_get()) { reset_filesystem = false; break; } sys_tick_delay_ms(10); } } #endif // local filesystem init { // try to mount the flash FRESULT res = f_mount(&fatfs0, "0:", 1); if (!reset_filesystem && res == FR_OK) { // mount sucessful } else if (reset_filesystem || res == FR_NO_FILESYSTEM) { // no filesystem, so create a fresh one // TODO doesn't seem to work correctly when reset_filesystem is true... // LED on to indicate creation of LFS led_state(PYB_LED_R2, 1); uint32_t stc = sys_tick_counter; res = f_mkfs("0:", 0, 0); if (res == FR_OK) { // success creating fresh LFS } else { __fatal_error("could not create LFS"); } // create src directory res = f_mkdir("0:/src"); // ignore result from mkdir // create empty main.py FIL fp; f_open(&fp, "0:/src/main.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(stc, 200); led_state(PYB_LED_R2, 0); } else { __fatal_error("could not access LFS"); } } // make sure we have a /boot.py { FILINFO fno; FRESULT res = f_stat("0:/boot.py", &fno); if (res == FR_OK) { if (fno.fattrib & AM_DIR) { // exists as a directory // TODO handle this case // see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation } else { // exists as a file, good! } } else { // doesn't exist, create fresh file // LED on to indicate creation of boot.py led_state(PYB_LED_R2, 1); uint32_t stc = sys_tick_counter; FIL fp; f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS); UINT n; f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n); // TODO check we could write n bytes f_close(&fp); // keep LED on for at least 200ms sys_tick_wait_at_least(stc, 200); led_state(PYB_LED_R2, 0); } } // run /boot.py if (!pyexec_file("0:/boot.py")) { flash_error(4); } if (first_soft_reset) { #if MICROPY_HW_HAS_MMA7660 // MMA accel: init and reset address to zero accel_init(); #endif } // turn boot-up LED off led_state(PYB_LED_G1, 0); #if MICROPY_HW_HAS_SDCARD // if an SD card is present then mount it on 1:/ if (sdcard_is_present()) { FRESULT res = f_mount(&fatfs1, "1:", 1); if (res != FR_OK) { printf("[SD] could not mount SD card\n"); } else { if (first_soft_reset) { // use SD card as medium for the USB MSD usbd_storage_select_medium(USBD_STORAGE_MEDIUM_SDCARD); } } } #endif #ifdef USE_HOST_MODE // USB host pyb_usb_host_init(); #elif defined(USE_DEVICE_MODE) // USB device pyb_usb_dev_init(PYB_USB_DEV_VCP_MSC); #endif // run main script { vstr_t *vstr = vstr_new(); vstr_add_str(vstr, "0:/"); if (pyb_config_source_dir == MP_OBJ_NULL) { vstr_add_str(vstr, "src"); } else { vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_source_dir)); } vstr_add_char(vstr, '/'); if (pyb_config_main == MP_OBJ_NULL) { vstr_add_str(vstr, "main.py"); } else { vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main)); } if (!pyexec_file(vstr_str(vstr))) { flash_error(3); } vstr_free(vstr); } #if MICROPY_HW_HAS_MMA7660 // HID example if (0) { uint8_t data[4]; data[0] = 0; data[1] = 1; data[2] = -2; data[3] = 0; for (;;) { #if MICROPY_HW_HAS_SWITCH if (switch_get()) { data[0] = 0x01; // 0x04 is middle, 0x02 is right } else { data[0] = 0x00; } #else data[0] = 0x00; #endif accel_start(0x4c /* ACCEL_ADDR */, 1); accel_send_byte(0); accel_restart(0x4c /* ACCEL_ADDR */, 0); for (int i = 0; i <= 1; i++) { int v = accel_read_ack() & 0x3f; if (v & 0x20) { v |= ~0x1f; } data[1 + i] = v; } accel_read_nack(); usb_hid_send_report(data); sys_tick_delay_ms(15); } } #endif #if MICROPY_HW_HAS_WLAN // wifi pyb_wlan_init(); pyb_wlan_start(); #endif pyexec_repl(); printf("PYB: sync filesystems\n"); storage_flush(); printf("PYB: soft reboot\n"); first_soft_reset = false; goto soft_reset; }
/* Main function * * - Initialise device and any global variables * - Enable Interrupts * - Start endless loop */ void main(void) { unsigned long ulRetcode; // Initialise the device clock to 80MHz ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN ); // Enable SysTick for FatFS at 10ms intervals ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 100); ROM_SysTickEnable(); ROM_SysTickIntEnable(); // // Configure and enable uDMA // ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UDMA); SysCtlDelay(10); ROM_uDMAControlBaseSet(&sDMAControlTable[0]); ROM_uDMAEnable(); // // Enable the USB controller. // ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_USB0); // // Set the USB pins to be controlled by the USB controller. ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); ROM_GPIOPinTypeUSBAnalog(GPIO_PORTD_BASE, GPIO_PIN_4 | GPIO_PIN_5); // Initialize the idle timeout and reset all flags. // g_ulIdleTimeout = 0; g_ulFlags = 0; // // Initialize the state to idle. // g_eMSCState = MSC_DEV_DISCONNECTED; // // Set the USB stack mode to Device mode with VBUS monitoring. // USBStackModeSet(0, USB_MODE_DEVICE, 0); // // Pass our device information to the USB library and place the device // on the bus. // USBDMSCInit(0, (tUSBDMSCDevice *)&g_sMSCDevice); // // Determine whether or not an SDCard is installed. If not, print a // warning and have the user install one and restart. // ulRetcode = disk_initialize(0); // Enable Global interrupts ROM_IntMasterEnable(); // Enable floating point arithmetic unit, but disable stacking ROM_FPUEnable(); ROM_FPUStackingDisable(); // Initialise GPIO - All ports enabled ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); // Unlock NMI pins for GPIO usage (PD7 + PF0) HWREG(GPIO_PORTD_BASE + 0x520) = 0x4C4F434B; HWREG(GPIO_PORTF_BASE + 0x520) = 0x4C4F434B; HWREG(GPIO_PORTD_BASE + 0x524) = 0x000000FF; HWREG(GPIO_PORTF_BASE + 0x524) = 0x000000FF; HWREG(GPIO_PORTD_BASE + 0x520) = 0x00000000; HWREG(GPIO_PORTF_BASE + 0x520) = 0x00000000; // Initialise GPIO Expander (probably not happening) // Initialise Buttons btn_init(); // Initialise FX fx_init(); // Initialise ADC adc_init(); // Initialise DAC dac_init(); // Initialise SD Card and Mass storage sdcard_init(); // Initialise UART for debugging uart_init(); // Initialise Timers timers_init(); for(;;) { // Endless Loop ADCProcessorTrigger(ADC0_BASE, 0); SysCtlDelay(SysCtlClockGet() / 120); // 25ms } }
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; }
DSTATUS disk_initialize ( BYTE drv /* Physical drive nmuber (0) */ ) { // BYTE n, cmd, ty, ocr[4]; UINT cmd, n; DWORD resp[4]; BYTE ty; SDC_PRMS_T params; if (!sdcard_init(¶ms)) { return STA_NOINIT; } if (drv) return STA_NOINIT; /* Supports only single drive */ if (Stat & STA_NODISK) return Stat; /* No card in the socket */ power_on(); /* Force socket power on */ for (Timer1 = 2; Timer1; ); send_cmd(CMD0, 0, 0, NULL); /* Enter idle state */ CardRCA = 0; Timer1 = 10; while(Timer1); /*---- Card is 'idle' state ----*/ Timer1 = 100; /* Initialization timeout of 1000 msec */ if (send_cmd(CMD8, 0x1AA, 1, resp) /* SDC Ver2 */ && (resp[0] & 0xFFF) == 0x1AA) { /* The card can work at vdd range of 2.7-3.6V */ do { /* Wait while card is busy state (use ACMD41 with HCS bit) */ /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */ if (!Timer1) goto di_fail; } while (!send_cmd(ACMD41, 0x40FF8000, 1, resp) || !(resp[0] & 0x80000000)); ty = (resp[0] & 0x40000000) ? CT_SD2|CT_BLOCK : CT_SD2; /* Check CCS bit in the OCR */ } else { /* SDC Ver1 or MMC */ if (send_cmd(ACMD41, /*0x00FF8000*/0, 1, resp)) { ty = CT_SD1; cmd = ACMD41; /* ACMD41 is accepted -> SDC Ver1 */ } else { ty = CT_MMC; cmd = CMD1; /* ACMD41 is rejected -> MMC */ } do { /* Wait while card is busy state (use ACMD41 or CMD1) */ /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */ if (!Timer1) { Timer1 = Timer1; goto di_fail; } } while (!send_cmd(cmd, 0x00FF8000, 1, resp) || !(resp[0] & 0x80000000)); } CardType = ty; /* Save card type */ bswap_cp(&CardInfo[32], resp); /* Save OCR */ /*---- Card is 'ready' state ----*/ if (!send_cmd(CMD2, 0, 2, resp)) goto di_fail; /* Enter ident state */ for (n = 0; n < 4; n++) bswap_cp(&CardInfo[n * 4 + 16], &resp[n]); /* Save CID */ /*---- Card is 'ident' state ----*/ if (ty & CT_SDC) { /* SDC: Get generated RCA and save it */ if (!send_cmd(CMD3, 0, 1, resp)) goto di_fail; CardRCA = (WORD)(resp[0] >> 16); } else { /* MMC: Assign RCA to the card */ if (!send_cmd(CMD3, 1 << 16, 1, resp)) goto di_fail;