自学教程：C++ up_udelay函数代码示例

intadc_init(void){	adc_perf = perf_alloc(PC_ELAPSED, "adc");	/* do calibration if supported */#ifdef ADC_CR2_CAL	rCR2 |= ADC_CR2_RSTCAL;	up_udelay(1);	if (rCR2 & ADC_CR2_RSTCAL)		return -1;	rCR2 |= ADC_CR2_CAL;	up_udelay(100);	if (rCR2 & ADC_CR2_CAL)		return -1;#endif	/* arbitrarily configure all channels for 55 cycle sample time */	rSMPR1 = 0b00000011011011011011011011011011;	rSMPR2 = 0b00011011011011011011011011011011;	/* XXX for F2/4, might want to select 12-bit mode? */	rCR1 = 0;	/* enable the temperature sensor / Vrefint channel if supported*/	rCR2 =#ifdef ADC_CR2_TSVREFE		/* enable the temperature sensor in CR2 */		ADC_CR2_TSVREFE |#endif		0;#ifdef ADC_CCR_TSVREFE	/* enable temperature sensor in CCR */	rCCR = ADC_CCR_TSVREFE;#endif	/* configure for a single-channel sequence */	rSQR1 = 0;	rSQR2 = 0;	rSQR3 = 0;	/* will be updated with the channel each tick */	/* power-cycle the ADC and turn it on */	rCR2 &= ~ADC_CR2_ADON;	up_udelay(10);	rCR2 |= ADC_CR2_ADON;	up_udelay(10);	rCR2 |= ADC_CR2_ADON;	up_udelay(10);	return 0;}

int BMI160::reset(){	write_reg(BMIREG_CONF, (1 << 1)); //Enable NVM programming	write_checked_reg(BMIREG_ACC_CONF,      BMI_ACCEL_US | BMI_ACCEL_BWP_NORMAL); //Normal operation, no decimation	write_checked_reg(BMIREG_ACC_RANGE,     0);	write_checked_reg(BMIREG_GYR_CONF,      BMI_GYRO_BWP_NORMAL);   //Normal operation, no decimation	write_checked_reg(BMIREG_GYR_RANGE,     0);	write_checked_reg(BMIREG_INT_EN_1,      BMI_DRDY_INT_EN); //Enable DRDY interrupt	write_checked_reg(BMIREG_INT_OUT_CTRL,  BMI_INT1_EN);   //Enable interrupts on pin INT1	write_checked_reg(BMIREG_INT_MAP_1,     BMI_DRDY_INT1); //DRDY interrupt on pin INT1	write_checked_reg(BMIREG_IF_CONF,       BMI_SPI_4_WIRE |			  BMI_AUTO_DIS_SEC); //Disable secondary interface; Work in SPI 4-wire mode	write_checked_reg(BMIREG_NV_CONF,       BMI_SPI); //Disable I2C interface	set_accel_range(BMI160_ACCEL_DEFAULT_RANGE_G);	accel_set_sample_rate(BMI160_ACCEL_DEFAULT_RATE);	set_gyro_range(BMI160_GYRO_DEFAULT_RANGE_DPS);	gyro_set_sample_rate(BMI160_GYRO_DEFAULT_RATE);	//_set_dlpf_filter(BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); //NOT CONSIDERING FILTERING YET	//Enable Accelerometer in normal mode	write_reg(BMIREG_CMD, BMI_ACCEL_NORMAL_MODE);	up_udelay(4100);	//usleep(4100);	//Enable Gyroscope in normal mode	write_reg(BMIREG_CMD, BMI_GYRO_NORMAL_MODE);	up_udelay(80300);	//usleep(80300);	uint8_t retries = 10;	while (retries--) {		bool all_ok = true;		for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {			if (read_reg(_checked_registers[i]) != _checked_values[i]) {				write_reg(_checked_registers[i], _checked_values[i]);				all_ok = false;			}		}		if (all_ok) {			break;		}	}	_accel_reads = 0;	_gyro_reads = 0;	return OK;}

/** * @brief Exchange one bit in mode 0 * * The function simulates SPI one bit transaction function in SPI mode 0. * * @param outdata data for SPI output * @param holdtime SPI hold time timing * @param loopback lookback mode supported or not * @return input data that read from SPI input pin */static uint8_t tsb_spi_bitexchange0(uint32_t outdata, uint32_t holdtime, bool loopback){    uint8_t indata = 0;    gpio_set_value(SPI_SDO, (outdata)? 1 : 0);    up_udelay(holdtime);    gpio_set_value(SPI_SCK, 1);    indata = gpio_get_value((loopback)? SPI_SDO : SPI_SDI);    up_udelay(holdtime);    gpio_set_value(SPI_SCK, 0);    return indata;}

intadc_init(void){    adc_perf = perf_alloc(PC_ELAPSED, "adc");    /* put the ADC into power-down mode */    rCR2 &= ~ADC_CR2_ADON;    up_udelay(10);    /* bring the ADC out of power-down mode */    rCR2 |= ADC_CR2_ADON;    up_udelay(10);    /* do calibration if supported */#ifdef ADC_CR2_CAL    rCR2 |= ADC_CR2_RSTCAL;    up_udelay(1);    if (rCR2 & ADC_CR2_RSTCAL)        return -1;    rCR2 |= ADC_CR2_CAL;    up_udelay(100);    if (rCR2 & ADC_CR2_CAL)        return -1;#endif    /*     * Configure sampling time.     *     * For electrical protection reasons, we want to be able to have     * 10K in series with ADC inputs that leave the board. At 12MHz this     * means we need 28.5 cycles of sampling time (per table 43 in the     * datasheet).     */    rSMPR1 = 0b00000000011011011011011011011011;    rSMPR2 = 0b00011011011011011011011011011011;    rCR2 |=	ADC_CR2_TSVREFE;		/* enable the temperature sensor / Vrefint channel */    /* configure for a single-channel sequence */    rSQR1 = 0;    rSQR2 = 0;    rSQR3 = 0;	/* will be updated with the channel at conversion time */    return 0;}

/** * @brief Manage the regulator state; Turn it on when needed * @returns: 0 on success, <0 on error */int vreg_get(struct vreg *vreg) {    unsigned int i, rc = 0;    if (!vreg) {        return -ENODEV;    }    dbg_verbose("%s %s/n", __func__, vreg->name ? vreg->name : "unknown");    /* Enable the regulator on the first use; Update use count */    if (atomic_inc(&vreg->use_count) == 1) {        for (i = 0; i < vreg->nr_vregs; i++) {            if (!&vreg->vregs[i]) {                rc = -EINVAL;                break;            }            dbg_insane("%s: %s vreg, gpio %d to %d, hold %dus/n", __func__,                       vreg->name ? vreg->name : "unknown",                       vreg->vregs[i].gpio, !!vreg->vregs[i].active_high,                       vreg->vregs[i].hold_time);            gpio_set_value(vreg->vregs[i].gpio, vreg->vregs[i].active_high);            up_udelay(vreg->vregs[i].hold_time);        }    }    /* Update state */    vreg->power_state = true;    return rc;}

__EXPORT int nsh_archinitialize(void){	/* the interruption subsystem is not initialized when stm32_boardinitialize() is called */	stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback);	/* configure power supply control/sense pins */	stm32_configgpio(GPIO_VDD_5V_SENSORS_EN);	/* configure the high-resolution time/callout interface */	hrt_init();	/* configure the DMA allocator */	dma_alloc_init();	/* configure CPU load estimation */#ifdef CONFIG_SCHED_INSTRUMENTATION	cpuload_initialize_once();#endif	/* set up the serial DMA polling */	static struct hrt_call serial_dma_call;	struct timespec ts;	/*	 * Poll at 1ms intervals for received bytes that have not triggered	 * a DMA event.	 */	ts.tv_sec = 0;	ts.tv_nsec = 1000000;	hrt_call_every(&serial_dma_call,		       ts_to_abstime(&ts),		       ts_to_abstime(&ts),		       (hrt_callout)stm32_serial_dma_poll,		       NULL);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	led_off(LED_BLUE);	/* Configure SPI-based devices */	spi1 = up_spiinitialize(1);	if (!spi1) {		message("[boot] FAILED to initialize SPI port 1/n");		up_ledon(LED_AMBER);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);	up_udelay(20);	return OK;}

/** * @brief Given a table of interfaces, power off all associated *        power supplies * @param interfaces table of interfaces to initialize * @param nr_ints number of interfaces to initialize * @param nr_spring_ints number of spring interfaces * @returns: 0 on success, <0 on error */int interface_early_init(struct interface **ints,                         size_t nr_ints, size_t nr_spring_ints) {    unsigned int i;    int rc;    int fail = 0;    dbg_info("Power off all interfaces/n");    if (!ints) {        return -ENODEV;    }    interfaces = ints;    nr_interfaces = nr_ints;    nr_spring_interfaces = nr_spring_ints;    for (i = 0; i < nr_interfaces; i++) {        rc = interface_config(interfaces[i]);        if (rc < 0) {            dbg_error("Failed to power interface %s/n", interfaces[i]->name);            fail = 1;            /* Continue configuring remaining interfaces */            continue;        }    }    if (fail) {        return -1;    }    /* Let everything settle for a good long while.*/    up_udelay(POWER_OFF_TIME_IN_US);    return 0;}

static uint16_t ads7843e_sendcmd(FAR struct ads7843e_dev_s *priv, uint8_t cmd){  uint8_t  buffer[2];  uint16_t result;  /* Select the ADS7843E */  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);  /* Send the command */  (void)SPI_SEND(priv->spi, cmd);  /* Wait a tiny amount to make sure that the aquisition time is complete */   up_udelay(3); /* 3 microseconds */  /* Read the 12-bit data (LS 4 bits will be padded with zero) */  SPI_RECVBLOCK(priv->spi, buffer, 2);  SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);  result = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];  result = result >> 4;  ivdbg("cmd:%02x response:%04x/n", cmd, result);  return result;}

void sam_sckc_enable(bool enable){  uint32_t regval;#ifdef ATSAMA5D3  /* REVISIT: Missing the logic that disables the external OSC32 */  /* Enable external OSC 32 kHz */  regval  = getreg32(SAM_SCKC_CR);  regval |= SCKC_CR_OSC32EN;  putreg32(regval, SAM_SCKC_CR);  /* Wait for 32,768 XTAL start-up time */  up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);  /* Disable OSC 32 kHz bypass */  regval &= ~SCKC_CR_OSC32BYP;  putreg32(regval, SAM_SCKC_CR);  /* Switch slow clock source to external OSC 32 kHz (*/  regval |= SCKC_CR_OSCSEL;  putreg32(regval, SAM_SCKC_CR);  /* Wait 5 slow clock cycles for internal resynchronization */  up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);  /* Disable internal RC 32 kHz */  regval &= ~SCKC_CR_RCEN;  putreg32(regval, SAM_SCKC_CR);#else  /* Switch slow clock source to external OSC 32 kHz */  regval = enable ? SCKC_CR_OSCSEL : 0;  putreg32(regval, SAM_SCKC_CR);  /* Wait 5 slow clock cycles for internal resynchronization */  up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);#endif}

intbma180_attach(struct spi_dev_s *spi, int spi_id){	int	result = ERROR;		bma180_dev.spi = spi;	bma180_dev.spi_id = spi_id;    	SPI_LOCK(bma180_dev.spi, true);	/* verify that the device is attached and functioning */	if (bma180_read_reg(ADDR_CHIP_ID) == CHIP_ID) {        		bma180_write_reg(ADDR_RESET, SOFT_RESET);       // page 48		up_udelay(13000);                               // wait 12 ms, see page 49		/* Configuring the BMA180 */		/* enable writing to chip config */		uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);		ctrl0 |= REG0_WRITE_ENABLE;		bma180_write_reg(ADDR_CTRL_REG0, ctrl0);		/* disable I2C interface, datasheet page 31 */		uint8_t disi2c = bma180_read_reg(ADDR_DIS_I2C);		disi2c |= 0x01;		bma180_write_reg(ADDR_DIS_I2C, disi2c);		/* block writing to chip config */		ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);		ctrl0 &= (~REG0_WRITE_ENABLE);		bma180_write_reg(ADDR_CTRL_REG0, ctrl0);		// up_udelay(500);		/* set rate */		result = bma180_set_rate(BMA180_RATE_LP_600HZ);		// up_udelay(500);		/* set range */		result += bma180_set_range(BMA180_RANGE_4G);		// up_udelay(500);		if (result == 0) {			/* make ourselves available */			register_driver("/dev/bma180", &bma180_fops, 0666, NULL);		}	} else {		errno = EIO;	}	SPI_LOCK(bma180_dev.spi, false);    	return result;}

static int determin_hw_version(int *version, int *revision){	*revision = 0; /* default revision */	int rv = 0;	int pos = 0;	stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));	up_udelay(10);	rv |= stm32_gpioread(HW_VER_PB4) << pos++;	stm32_configgpio(HW_VER_PB4);	up_udelay(10);	rv |= stm32_gpioread(HW_VER_PB4) << pos++;	int votes = 16;	int ones[2] = {0, 0};	int zeros[2] = {0, 0};	while (votes--) {		stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB12 & ~GPIO_PUPD_MASK));		up_udelay(10);		stm32_gpioread(HW_VER_PB12) ? ones[0]++ : zeros[0]++;		stm32_configgpio(HW_VER_PB12);		up_udelay(10);		stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;	}	if (ones[0] > zeros[0]) {		rv |= 1 << pos;	}	pos++;	if (ones[1] > zeros[1]) {		rv |= 1 << pos;	}	stm32_configgpio(HW_VER_PB4_INIT);	stm32_configgpio(HW_VER_PB12_INIT);	*version = rv;	return OK;}

/* (init as in... make it accessible for the app - the driver  * itself is initialized in nsh_archinitialize() in up_nsh.c)  */void helper_init_adc(void){  /* open the ADC file */  adc_fd = open( ORCACTRL_ADC_FILE, O_RDONLY | O_NONBLOCK );  if( adc_fd < 0 )  /* fd < 0 means error opening the file */  { /* no other error handling here at the moment */    printf( "failed opening " ORCACTRL_ADC_FILE "/n" );  }  /* wait a while to make sure it is started properly */  up_udelay(10000); /* numerical value taken from ADC test app,                      * most propably empirical */}

static void spi_sndblock(FAR struct spi_dev_s *dev, FAR const void *buffer, size_t nwords){    FAR const uint8_t *ptr = (FAR const uint8_t *)buffer;    uint8_t sr;    /* Loop while thre are bytes remaining to be sent */    spidbg("nwords: %d/n", nwords);    while (nwords > 0)    {        /* While the TX FIFO is not full and there are bytes left to send */        while ((getreg8(LPC214X_SPI1_SR) & LPC214X_SPI1SR_TNF) && nwords)        {            /* Send the data */            putreg16((uint16_t)*ptr, LPC214X_SPI1_DR);            ptr++;            nwords--;        }    }    /* Then discard all card responses until the RX & TX FIFOs are emptied. */    spidbg("discarding/n");    do    {        /* Is there anything in the RX fifo? */        sr = getreg8(LPC214X_SPI1_SR);        if ((sr & LPC214X_SPI1SR_RNE) != 0)        {            /* Yes.. Read and discard */            (void)getreg16(LPC214X_SPI1_DR);        }        /* There is a race condition where TFE may go true just before         * RNE goes true and this loop terminates prematurely.  The nasty little         * delay in the following solves that (it could probably be tuned         * to improve performance).         */        else if ((sr & LPC214X_SPI1SR_TFE) != 0)        {            up_udelay(100);            sr = getreg8(LPC214X_SPI1_SR);        }    }    while ((sr & LPC214X_SPI1SR_RNE) != 0 || (sr & LPC214X_SPI1SR_TFE) == 0);}

FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno){  FAR struct spi_dev_s *spi;  FAR struct lcd_dev_s *dev;  /* Configure the LCD GPIOs */  lcd_dumpgpio("up_nxdrvinit: On entry");  lpc17_configgpio(LPC1766STK_LCD_RST);  lpc17_configgpio(LPC1766STK_LCD_BL);  lcd_dumpgpio("up_nxdrvinit: After GPIO setup");  /* Reset the LCD */  lpc17_gpiowrite(LPC1766STK_LCD_RST, false);  up_udelay(10);  lpc17_gpiowrite(LPC1766STK_LCD_RST, true);  up_mdelay(5);  /* Configure PWM1 to support the backlight */  nokia_blinitialize();  /* Get the SSP port (configure as a Freescale SPI port) */  spi = up_spiinitialize(0);  if (!spi)    {      glldbg("Failed to initialize SSP port 0/n");    }  else    {      /* Bind the SSP port to the LCD */      dev = nokia_lcdinitialize(spi, devno);      if (!dev)        {          glldbg("Failed to bind SSP port 0 to LCD %d: %d/n", devno);        }     else        {          gllvdbg("Bound SSP port 0 to LCD %d/n", devno);          /* And turn the LCD on (CONFIG_LCD_MAXPOWER should be 1) */          (void)dev->setpower(dev, CONFIG_LCD_MAXPOWER);          return dev;        }    }  return NULL;}

int BMI055_accel::reset(){	write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset	up_udelay(5000);	write_checked_reg(BMI055_ACC_BW,    BMI055_ACCEL_BW_1000); //Write accel bandwidth	write_checked_reg(BMI055_ACC_RANGE,     BMI055_ACCEL_RANGE_2_G);//Write range	write_checked_reg(BMI055_ACC_INT_EN_1,      BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt	write_checked_reg(BMI055_ACC_INT_MAP_1,     BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1	set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range	accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR	//Enable Accelerometer in normal mode	write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL);	up_udelay(1000);	uint8_t retries = 10;	while (retries--) {		bool all_ok = true;		for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {			if (read_reg(_checked_registers[i]) != _checked_values[i]) {				write_reg(_checked_registers[i], _checked_values[i]);				all_ok = false;			}		}		if (all_ok) {			break;		}	}	_accel_reads = 0;	return OK;}

/* EVT1 */static int evt1_board_init(struct ara_board_info *board_info) {    int rc;    /* For now, just always enable REFCLK_MAIN and the buffers. */    rc = vreg_config(&refclk_main_vreg) || vreg_get(&refclk_main_vreg);    if (rc) {        dbg_error("%s: can't start REFCLK_MAIN: %d/n", __func__, rc);        return ERROR;    }    /* Configure the switch power supply lines. */    rc = vreg_config(&sw_vreg);    if (rc) {        dbg_error("%s: can't configure switch regulators: %d/n", __func__, rc);        return ERROR;    }    stm32_configgpio(evt1_board_info.sw_data.gpio_reset);    up_udelay(POWER_SWITCH_OFF_STAB_TIME_US);    /* Configure the wake/detect lines. */    stm32_configgpio(WD_1_DET_IN_GPIO);    stm32_configgpio(WD_2_DET_IN_GPIO);    stm32_configgpio(WD_3A_DET_IN_GPIO);    stm32_configgpio(WD_3B_DET_IN_GPIO);    stm32_configgpio(WD_4A_DET_IN_GPIO);    stm32_configgpio(WD_4B_DET_IN_GPIO);    stm32_configgpio(WD_5_DET_IN_GPIO);    stm32_configgpio(WD_8A_DET_IN_GPIO);    stm32_configgpio(WD_8B_DET_IN_GPIO);    /* Configure the module release pins */    stm32_configgpio(MOD_RELEASE_1_CONFIG);    stm32_configgpio(MOD_RELEASE_2_CONFIG);    stm32_configgpio(MOD_RELEASE_3A_CONFIG);    stm32_configgpio(MOD_RELEASE_3B_CONFIG);    stm32_configgpio(MOD_RELEASE_4A_CONFIG);    stm32_configgpio(MOD_RELEASE_4B_CONFIG);    stm32_configgpio(MOD_RELEASE_5_CONFIG);    /* Configure ARA key input pin */    stm32_configgpio(ARA_KEY_CONFIG);    /*     * (Module hotplug pins unconfigured. TODO, part of SW-1942.)     */    return 0;}

intSPI::_transferword(uint16_t *send, uint16_t *recv, unsigned len){	SPI_SETFREQUENCY(_dev, _frequency);	SPI_SETMODE(_dev, _mode);	SPI_SETBITS(_dev, 16);							/* 16 bit transfer */	SPI_SELECT(_dev, _device, true);	/* do the transfer */	//SPI_EXCHANGE(_dev, send, recv, len);			/* Try to be compatible with the t_stall of the ADIS16448 which is 9usec (by applying 5usec delay ) */	SPI_EXCHANGE(_dev, send, nullptr, 1);	up_udelay(5);									/* Reduced to 5 usec (from 10 use) */	SPI_EXCHANGE(_dev, nullptr, recv+1, len-1);	////	/* and clean up */	SPI_SELECT(_dev, _device, false);	return OK;}

int BMI055_gyro::reset(){	write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset	usleep(5000);	write_checked_reg(BMI055_GYR_BW,     0); // Write Gyro Bandwidth	write_checked_reg(BMI055_GYR_RANGE,     0);// Write Gyro range	write_checked_reg(BMI055_GYR_INT_EN_0,      BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt	write_checked_reg(BMI055_GYR_INT_MAP_1,     BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1	set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range	gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR	//Enable Gyroscope in normal mode	write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL);	up_udelay(1000);	uint8_t retries = 10;	while (retries--) {		bool all_ok = true;		for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {			if (read_reg(_checked_registers[i]) != _checked_values[i]) {				write_reg(_checked_registers[i], _checked_values[i]);				all_ok = false;			}		}		if (all_ok) {			break;		}	}	_gyro_reads = 0;	return OK;}

void stm32_pwr_enablebkp(bool writable){  uint16_t regval;  irqstate_t flags;  flags = irqsave();  /* Enable or disable the ability to write*/  regval  = stm32_pwr_getreg(STM32_PWR_CR_OFFSET);  regval &= ~PWR_CR_DBP;  regval |= writable ? PWR_CR_DBP : 0;  stm32_pwr_putreg(STM32_PWR_CR_OFFSET, regval);  irqrestore(flags);  if (writable)    {      /* Enable does not happen right away */      up_udelay(4);    }}

//.........这里部分代码省略.........					case 'b':						dbgContinue = true;						break;					default:						break;					} // Inner Switch					message("/nEnter B - Continue booting/n" /						"Enter C - Clear the fault log/n" /						"Enter D - Dump fault log/n/n?>");					fflush(stdout);					if (!dbgContinue) {						c = getchar();					}					break;				} // outer switch			} // for		} // inner if	} // outer if#endif // CONFIG_STM32_SAVE_CRASHDUMP#endif // CONFIG_STM32_BBSRAM	/* initial LED state */	drv_led_start();	led_off(LED_RED);	led_off(LED_GREEN);	led_off(LED_BLUE);	/* Configure SPI-based devices */	spi1 = stm32_spibus_initialize(1);	if (!spi1) {		message("[boot] FAILED to initialize SPI port 1/n");		board_autoled_on(LED_RED);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);	SPI_SELECT(spi1, PX4_SPIDEV_HMC, false);	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);	up_udelay(20);	/* Get the SPI port for the FRAM */	spi2 = stm32_spibus_initialize(2);	if (!spi2) {		message("[boot] FAILED to initialize SPI port 2/n");		board_autoled_on(LED_RED);		return -ENODEV;	}	/* Default SPI2 to 12MHz and de-assert the known chip selects.	 * MS5611 has max SPI clock speed of 20MHz	 */	// XXX start with 10.4 MHz and go up to 20 once validated	SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000);	SPI_SETBITS(spi2, 8);	SPI_SETMODE(spi2, SPIDEV_MODE3);	SPI_SELECT(spi2, SPIDEV_FLASH, false);	SPI_SELECT(spi2, PX4_SPIDEV_BARO, false);#ifdef CONFIG_MMCSD	/* First, get an instance of the SDIO interface */	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);	if (!sdio) {		message("[boot] Failed to initialize SDIO slot %d/n",			CONFIG_NSH_MMCSDSLOTNO);		return -ENODEV;	}	/* Now bind the SDIO interface to the MMC/SD driver */	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);	if (ret != OK) {		message("[boot] Failed to bind SDIO to the MMC/SD driver: %d/n", ret);		return ret;	}	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */	sdio_mediachange(sdio, true);#endif	return OK;}

__EXPORT int nsh_archinitialize(void){	int result;	message("/n");	/* configure always-on ADC pins */	stm32_configgpio(GPIO_ADC1_IN0);	stm32_configgpio(GPIO_ADC1_IN10);	stm32_configgpio(GPIO_ADC1_IN11);	stm32_configgpio(GPIO_UART_SBUS_INVERTER);#ifdef CONFIG_RC_INPUTS_TYPE(RC_INPUT_SBUS)	stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 1);#else	stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 0);#endif	/* configure the high-resolution time/callout interface */	hrt_init();	/* configure CPU load estimation */#ifdef CONFIG_SCHED_INSTRUMENTATION	cpuload_initialize_once();#endif	/* initial BUZZER state */	drv_buzzer_start();	buzzer_off(BUZZER_EXT);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	led_off(LED_BLUE);	led_off(LED_GREEN);	led_off(LED_EXT1);	led_off(LED_EXT2);	/* Configure SPI-based devices */	message("[boot] Initializing SPI port 1/n");	spi1 = up_spiinitialize(1);	if (!spi1) {		message("[boot] FAILED to initialize SPI port 1/r/n");		led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, GPIO_SPI_CS_MS5611, false);	SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MS5611, false);	SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MPU6000, false);	SPI_SELECT(spi1, GPIO_SPI_CS_EXP_HMC5983, false);	SPI_SELECT(spi1, GPIO_SPI_CS_EXP_WIFI_EEPROM, false);	up_udelay(20);	message("[boot] Successfully initialized SPI port 1/r/n");//	message("[boot] Initializing Wireless Module/n");//	wireless_archinitialize();	message("[boot] Initializing SPI port 2/n");	spi2 = up_spiinitialize(2);	if (!spi2) {		message("[boot] FAILED to initialize SPI port 2/r/n");		led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI2 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi2, 10000000);	SPI_SETBITS(spi2, 8);	SPI_SETMODE(spi2, SPIDEV_MODE3);	SPI_SELECT(spi2, GPIO_SPI_CS_MPU6000, false);	SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MS5611, false);	SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MPU6000, false);//.........这里部分代码省略.........

//.........这里部分代码省略.........	/* add a performance counter for controls */	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");	/* and one for measuring the loop rate */	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");	struct mallinfo minfo = mallinfo();	lowsyslog("MEM: free %u, largest %u/n", minfo.mxordblk, minfo.fordblks);	/* initialize PWM limit lib */	pwm_limit_init(&pwm_limit);	/*	 *    P O L I C E    L I G H T S	 *	 * Not enough memory, lock down.	 *	 * We might need to allocate mixers later, and this will	 * ensure that a developer doing a change will notice	 * that he just burned the remaining RAM with static	 * allocations. We don't want him to be able to	 * get past that point. This needs to be clearly	 * documented in the dev guide.	 *	 */	if (minfo.mxordblk < 600) {		lowsyslog("ERR: not enough MEM");		bool phase = false;		while (true) {			if (phase) {				LED_AMBER(true);				LED_BLUE(false);			} else {				LED_AMBER(false);				LED_BLUE(true);			}			up_udelay(250000);			phase = !phase;		}	}	/* Start the failsafe led init */	failsafe_led_init();	/*	 * Run everything in a tight loop.	 */	uint64_t last_debug_time = 0;	uint64_t last_heartbeat_time = 0;	for (;;) {		/* track the rate at which the loop is running */		perf_count(loop_perf);		/* kick the mixer */		perf_begin(mixer_perf);		mixer_tick();		perf_end(mixer_perf);		/* kick the control inputs */		perf_begin(controls_perf);		controls_tick();		perf_end(controls_perf);		if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) {			last_heartbeat_time = hrt_absolute_time();			heartbeat_blink();		}		ring_blink();		check_reboot();		/* check for debug activity (default: none) */		show_debug_messages();		/* post debug state at ~1Hz - this is via an auxiliary serial port		 * DEFAULTS TO OFF!		 */		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],				  (unsigned)r_status_flags,				  (unsigned)r_setup_arming,				  (unsigned)r_setup_features,				  (unsigned)mallinfo().mxordblk);			last_debug_time = hrt_absolute_time();		}	}}

int MPU9250::reset(){	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);	up_udelay(10000);	write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);	up_udelay(1000);	write_checked_reg(MPUREG_PWR_MGMT_2, 0);	up_udelay(1000);	// SAMPLE RATE	_set_sample_rate(_sample_rate);	usleep(1000);	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)	// was 90 Hz, but this ruins quality and does not improve the	// system response	_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);	usleep(1000);	// Gyro scale 2000 deg/s ()	write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);	usleep(1000);	// correct gyro scale factors	// scale to rad/s in SI units	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s	// scaling factor:	// 1/(2^15)*(2000/180)*PI	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;	set_accel_range(16);	usleep(1000);	// INT CFG => Interrupt on Data Ready	write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready	usleep(1000);#ifdef USE_I2C	bool bypass = !_mag->is_passthrough();#else	bool bypass = false;#endif	write_checked_reg(MPUREG_INT_PIN_CFG,			  BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN :					  0)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed	usleep(1000);	write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);	usleep(1000);	uint8_t retries = 10;	while (retries--) {		bool all_ok = true;		for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {			if (read_reg(_checked_registers[i]) != _checked_values[i]) {				write_reg(_checked_registers[i], _checked_values[i]);				all_ok = false;			}		}		if (all_ok) {			break;		}	}	return OK;}

int up_lcd1602_initialize(void){  uint32_t regval;  int ret = OK;  /* Only initialize the driver once. */  if (!g_lcd1602.initialized)    {      lcdvdbg("Initializing/n");      /* PMP Master mode configuration */      /* Make sure that interrupts are disabled */      putreg32(INT_PMP, PIC32MX_INT_IEC1CLR);      /* Stop and reset the PMP module and clear the mode and control registers. */      putreg32(0, PIC32MX_PMP_MODE);      putreg32(0, PIC32MX_PMP_AEN);      putreg32(0, PIC32MX_PMP_CON);      putreg32(0, PIC32MX_PMP_ADDR);      /* Set LCD timing values, PMP master mode 3, 8-bit mode, no address       * increment, and no interrupts.       */      regval = (PMP_MODE_WAITE_RD(0) | PMP_MODE_WAITM(3) | PMP_MODE_WAITB_1TPB |                PMP_MODE_MODE_MODE1 | PMP_MODE_MODE8 | PMP_MODE_INCM_NONE |                PMP_MODE_IRQM_NONE);      putreg32(regval, PIC32MX_PMP_MODE);      /* Enable the PMP for reading and writing       *   PMRD/PMWR is active high (1=RD; 0=WR)       *   PMENB is active high.       *   No chip selects       *   Address latch is active high       *   Enable PMRD/PMWR, PMENB, and the PMP.       */      regval = (PMP_CON_RDSP | PMP_CON_WRSP | PMP_CON_ALP |                PMP_CON_CSF_ADDR1415 | PMP_CON_PTRDEN | PMP_CON_PTWREN |                PMP_CON_ADRMUX_NONE | PMP_CON_ON);      putreg32(regval, PIC32MX_PMP_CON);      /* Configure and enable the LCD */      /* Wait > 15 milliseconds afer Vdd > 4.5V */      up_mdelay(100);      /* Select the 8-bit interface. BF cannot be checked before this command.       * This needs to be done a few times with some magic delays.       */      lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);      up_mdelay(50);      lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);      up_udelay(50);      lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);      lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);      /* Configure the display */      lcd_wrcommand(HD4478OU_DISPLAY);                       /* Display, cursor, and blink off */      lcd_wrcommand(HD4478OU_CLEAR);                         /* Clear the display */      lcd_wrcommand(HD4478OU_INPUT | HD4478OU_INPUT_INCR);   /* Increment mode */      lcd_wrcommand(HD4478OU_DISPLAY | HD4478OU_DISPLAY_ON); /* Display on, cursor and blink off */      lcd_wrcommand(HD4478OU_DDRAM_AD(0));                   /* Select DDRAM RAM AD=0 */      /* Register the LCD device driver */      ret = register_driver("/dev/lcd1602", &g_lcdops, 0644, &g_lcd1602);      g_lcd1602.initialized = true;    }  return ret;}

__EXPORT int nsh_archinitialize(void){	int result;	message("/n");	/* configure always-on ADC pins */	stm32_configgpio(GPIO_ADC1_IN1);	stm32_configgpio(GPIO_ADC1_IN2);	stm32_configgpio(GPIO_ADC1_IN3);	stm32_configgpio(GPIO_ADC1_IN10);	/* configure the high-resolution time/callout interface */	hrt_init();	/* configure the DMA allocator */	dma_alloc_init();	/* configure CPU load estimation */#ifdef CONFIG_SCHED_INSTRUMENTATION	cpuload_initialize_once();#endif	/* set up the serial DMA polling */	static struct hrt_call serial_dma_call;	struct timespec ts;	/*	 * Poll at 1ms intervals for received bytes that have not triggered	 * a DMA event.	 */	ts.tv_sec = 0;	ts.tv_nsec = 1000000;	hrt_call_every(&serial_dma_call,		       ts_to_abstime(&ts),		       ts_to_abstime(&ts),		       (hrt_callout)stm32_serial_dma_poll,		       NULL);	/* initial BUZZER state */	drv_buzzer_start();	buzzer_off(BUZZER_EXT);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	led_off(LED_BLUE);	led_off(LED_GREEN);	led_off(LED_EXT1);	led_off(LED_EXT2);	/* Configure SPI-based devices */	message("[boot] Initializing SPI port 1/n");	spi1 = up_spiinitialize(1);	if (!spi1) {		message("[boot] FAILED to initialize SPI port 1/r/n");		led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, SPIDEV_WIRELESS, false);	SPI_SELECT(spi1, SPIDEV_MS5611, false);	up_udelay(20);	message("[boot] Successfully initialized SPI port 1/r/n");	message("[boot] Initializing SPI port 2/n");	spi2 = up_spiinitialize(2);	if (!spi2) {		message("[boot] FAILED to initialize SPI port 2/r/n");		led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI2 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi2, 10000000);	SPI_SETBITS(spi2, 8);	SPI_SETMODE(spi2, SPIDEV_MODE3);	SPI_SELECT(spi2, SPIDEV_MPU6000, false);	message("[boot] Successfully initialized SPI port 2/n");	/* Get the SPI port for the microSD slot */	message("[boot] Initializing SPI port 3/n");	spi3 = up_spiinitialize(3);	if (!spi3) {		message("[boot] FAILED to initialize SPI port 3/n");//.........这里部分代码省略.........

int nsh_archinitialize(void){  int result;  /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */  /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */  /* configure the high-resolution time/callout interface */#ifdef CONFIG_HRT_TIMER  hrt_init();#endif  /* configure CPU load estimation */  #ifdef CONFIG_SCHED_INSTRUMENTATION  cpuload_initialize_once();  #endif  /* set up the serial DMA polling */#ifdef SERIAL_HAVE_DMA  {    static struct hrt_call serial_dma_call;    struct timespec ts;    /*      * Poll at 1ms intervals for received bytes that have not triggered     * a DMA event.     */    ts.tv_sec = 0;    ts.tv_nsec = 1000000;    hrt_call_every(&serial_dma_call,                    ts_to_abstime(&ts),                   ts_to_abstime(&ts),                   (hrt_callout)stm32_serial_dma_poll,                   NULL);  }#endif  message("/r/n");  up_ledoff(LED_BLUE);  up_ledoff(LED_AMBER);  up_ledon(LED_BLUE);  /* Configure user-space led driver */  px4fmu_led_init();  /* Configure SPI-based devices */  spi1 = up_spiinitialize(1);  if (!spi1)  {	  message("[boot] FAILED to initialize SPI port 1/r/n");	  up_ledon(LED_AMBER);	  return -ENODEV;  }  // Setup 10 MHz clock (maximum rate the BMA180 can sustain)  SPI_SETFREQUENCY(spi1, 10000000);  SPI_SETBITS(spi1, 8);  SPI_SETMODE(spi1, SPIDEV_MODE3);  SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);  SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);  SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);  up_udelay(20);  message("[boot] Successfully initialized SPI port 1/r/n");  /* initialize SPI peripherals redundantly */  int gyro_attempts = 0;  int gyro_fail = 0;  while (gyro_attempts < 5)  {	  gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO);	  gyro_attempts++;	  if (gyro_fail == 0) break;	  up_udelay(1000);  }  if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro/r/n");  int acc_attempts = 0;  int acc_fail = 0;  while (acc_attempts < 5)  {	  acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL);	  acc_attempts++;	  if (acc_fail == 0) break;	  up_udelay(1000);  }  if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer/r/n");  int mpu_attempts = 0;  int mpu_fail = 0;//.........这里部分代码省略.........

/** * Hold the usb hub under reset * * @param dev Device * @return 0 if successful */static int usb3813_hold_reset(struct device *dev){    gpio_direction_out(HUB_LINE_N_RESET, 0);    up_udelay(HUB_RESET_ASSERTION_TIME_IN_USEC);    return 0;}

__EXPORT int nsh_archinitialize(void){	/* configure ADC pins */	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */	stm32_configgpio(GPIO_ADC1_IN11);	/* BATT2_VOLTAGE_SENS */	stm32_configgpio(GPIO_ADC1_IN13);	/* BATT2_CURRENT_SENS */	/* configure power supply control/sense pins */	stm32_configgpio(GPIO_VDD_3V3_PERIPH_EN);	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);	stm32_configgpio(GPIO_VDD_5V_HIPOWER_EN);	stm32_configgpio(GPIO_VDD_BRICK_VALID);	stm32_configgpio(GPIO_VDD_BRICK2_VALID);	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);	stm32_configgpio(GPIO_VBUS_VALID);//	stm32_configgpio(GPIO_SBUS_INV);//	stm32_configgpio(GPIO_8266_GPIO0);//	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);//	stm32_configgpio(GPIO_8266_PD);//	stm32_configgpio(GPIO_8266_RST);//	stm32_configgpio(GPIO_BTN_SAFETY_FMU);	/* configure the GPIO pins to outputs and keep them low */	stm32_configgpio(GPIO_GPIO0_OUTPUT);	stm32_configgpio(GPIO_GPIO1_OUTPUT);	stm32_configgpio(GPIO_GPIO2_OUTPUT);	stm32_configgpio(GPIO_GPIO3_OUTPUT);	stm32_configgpio(GPIO_GPIO4_OUTPUT);	stm32_configgpio(GPIO_GPIO5_OUTPUT);	/* configure the high-resolution time/callout interface */	hrt_init();	/* configure the DMA allocator */	dma_alloc_init();	/* configure CPU load estimation */#ifdef CONFIG_SCHED_INSTRUMENTATION	cpuload_initialize_once();#endif	/* set up the serial DMA polling */	static struct hrt_call serial_dma_call;	struct timespec ts;	/*	 * Poll at 1ms intervals for received bytes that have not triggered	 * a DMA event.	 */	ts.tv_sec = 0;	ts.tv_nsec = 1000000;	hrt_call_every(&serial_dma_call,		       ts_to_abstime(&ts),		       ts_to_abstime(&ts),		       (hrt_callout)stm32_serial_dma_poll,		       NULL);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	/* Configure SPI-based devices */	spi1 = up_spiinitialize(1);	if (!spi1) {		message("[boot] FAILED to initialize SPI port 1/n");		up_ledon(LED_AMBER);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, PX4_SPIDEV_ICM, false);	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);	SPI_SELECT(spi1, PX4_SPIDEV_LIS, false);	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);	SPI_SELECT(spi1, PX4_SPIDEV_EEPROM, false);	up_udelay(20);	/* Get the SPI port for the FRAM */	spi2 = up_spiinitialize(2);	if (!spi2) {		message("[boot] FAILED to initialize SPI port 2/n");		up_ledon(LED_AMBER);		return -ENODEV;	}//.........这里部分代码省略.........

__EXPORT int nsh_archinitialize(void){	/* configure ADC pins */	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */	/* configure power supply control/sense pins */	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);	stm32_configgpio(GPIO_VDD_BRICK_VALID);	stm32_configgpio(GPIO_VDD_SERVO_VALID);	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)	/* run C++ ctors before we go any further */	up_cxxinitialize();#	if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)#  		error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.#	endif#else#  error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.#endif	/* configure the high-resolution time/callout interface */	hrt_init();	/* configure the DMA allocator */	dma_alloc_init();	/* configure CPU load estimation */#ifdef CONFIG_SCHED_INSTRUMENTATION	cpuload_initialize_once();#endif	/* set up the serial DMA polling */	static struct hrt_call serial_dma_call;	struct timespec ts;	/*	 * Poll at 1ms intervals for received bytes that have not triggered	 * a DMA event.	 */	ts.tv_sec = 0;	ts.tv_nsec = 1000000;	hrt_call_every(&serial_dma_call,		       ts_to_abstime(&ts),		       ts_to_abstime(&ts),		       (hrt_callout)stm32_serial_dma_poll,		       NULL);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	/* Configure SPI-based devices */	spi1 = up_spiinitialize(1);	if (!spi1) {		syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1/n");		board_led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI1 to 1MHz and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi1, 10000000);	SPI_SETBITS(spi1, 8);	SPI_SETMODE(spi1, SPIDEV_MODE3);	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);	up_udelay(20);	syslog(LOG_INFO, "[boot] Initialized SPI port 1 (SENSORS)/n");	/* Get the SPI port for the FRAM */	spi2 = up_spiinitialize(2);	if (!spi2) {		syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2/n");		board_led_on(LED_AMBER);		return -ENODEV;	}	/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)	 * and de-assert the known chip selects. *///.........这里部分代码省略.........

//.........这里部分代码省略.........		log("Failed to stat %s - %d/n", filename, (int)errno);		tcsetattr(_io_fd, TCSANOW, &t_original);		close(_io_fd);		_io_fd = -1;		return -errno;	}	fw_size = st.st_size;	if (_fw_fd == -1) {		tcsetattr(_io_fd, TCSANOW, &t_original);		close(_io_fd);		_io_fd = -1;		return -ENOENT;	}	/* do the usual program thing - allow for failure */	for (unsigned retries = 0; retries < 1; retries++) {		if (retries > 0) {			log("retrying update...");			ret = sync();			if (ret != OK) {				/* this is immediately fatal */				log("bootloader not responding");				tcsetattr(_io_fd, TCSANOW, &t_original);				close(_io_fd);				_io_fd = -1;				return -EIO;			}		}		ret = get_info(INFO_BL_REV, bl_rev);		if (ret == OK) {			if (bl_rev <= BL_REV) {				log("found bootloader revision: %d", bl_rev);			} else {				log("found unsupported bootloader revision %d, exiting", bl_rev);				tcsetattr(_io_fd, TCSANOW, &t_original);				close(_io_fd);				_io_fd = -1;				return OK;			}		}		ret = erase();		if (ret != OK) {			log("erase failed");			continue;		}		ret = program(fw_size);		if (ret != OK) {			log("program failed");			continue;		}		if (bl_rev <= 2) {			ret = verify_rev2(fw_size);		} else {			/* verify rev 3 and higher. Every version *needs* to be verified. */			ret = verify_rev3(fw_size);		}		if (ret != OK) {			log("verify failed");			continue;		}		ret = reboot();		if (ret != OK) {			log("reboot failed");			tcsetattr(_io_fd, TCSANOW, &t_original);			close(_io_fd);			_io_fd = -1;			return ret;		}		log("update complete");		ret = OK;		break;	}	/* reset uart to previous/default baudrate */	tcsetattr(_io_fd, TCSANOW, &t_original);	close(_fw_fd);	close(_io_fd);	_io_fd = -1;	// sleep for enough time for the IO chip to boot. This makes	// forceupdate more reliably startup IO again after update	up_udelay(100*1000);	return ret;}