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

__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;}

uint32_t *isr_handler(uint32_t *regs){#ifdef CONFIG_SUPPRESS_INTERRUPTS  up_ledon(LED_INIRQ);  PANIC(OSERR_ERREXCEPTION); /* Doesn't return */  return regs;               /* To keep the compiler happy */#else  uint32_t *ret;  /* Dispatch the interrupt */  up_ledon(LED_INIRQ);  ret = common_handler((int)regs[REG_IRQNO], regs);  up_ledoff(LED_INIRQ);  return ret;#endif}

void up_sigdeliver(void){#ifndef CONFIG_DISABLE_SIGNALS  FAR struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;  chipreg_t regs[XCPTCONTEXT_REGS];  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably   * EINTR).   */  int saved_errno = rtcb->pterrno;  up_ledon(LED_SIGNAL);  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",       rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  z80_copystate(regs, rtcb->xcp.regs);  regs[XCPT_PC] = rtcb->xcp.saved_pc;  regs[XCPT_I]  = rtcb->xcp.saved_i;  /* Get a local copy of the sigdeliver function pointer.  We do this so   * that we can nullify the sigdeliver function pointer in the TCB and   * accept more signal deliveries while processing the current pending   * signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state. */  irqrestore(regs[XCPT_I]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sdbg("Resuming/n");  (void)irqsave();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of execution. */  up_ledoff(LED_SIGNAL);  z80_restoreusercontext(regs);#endif}

void up_initialize(void){  /* Initialize global variables */  current_regs = NULL;  /* Calibrate the timing loop */  up_calibratedelay();  /* Add any extra memory fragments to the memory manager */  up_addregion();  /* Initialize the interrupt subsystem */  up_irqinitialize();  /* Initialize the DMA subsystem if the weak function stm32_dmainitialize has been   * brought into the build   */#ifdef CONFIG_ARCH_DMA#ifdef CONFIG_HAVE_WEAKFUNCTIONS  if (up_dmainitialize)#endif    {      up_dmainitialize();    }#endif  /* Initialize the system timer interrupt */#if !defined(CONFIG_SUPPRESS_INTERRUPTS) && !defined(CONFIG_SUPPRESS_TIMER_INTS)  up_timerinit();#endif  /* Register devices */#if CONFIG_NFILE_DESCRIPTORS > 0  devnull_register();   /* Standard /dev/null */#endif  /* Initialize the serial device driver */#ifdef CONFIG_USE_SERIALDRIVER  up_serialinit();#endif  /* Initialize the netwok */  up_netinitialize();  /* Initialize USB -- device and/or host */  up_usbinitialize();  up_ledon(LED_IRQSENABLED);}

void up_sigdeliver(void){  _TCB  *rtcb = (_TCB*)g_readytorun.head;  uint32_t regs[XCPTCONTEXT_REGS];  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably   * EINTR).   */  int saved_errno = rtcb->pterrno;  up_ledon(LED_SIGNAL);  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_PC]         = rtcb->xcp.saved_pc;  regs[REG_PRIMASK]    = rtcb->xcp.saved_primask;  regs[REG_XPSR]       = rtcb->xcp.saved_xpsr;  /* Get a local copy of the sigdeliver function pointer. We do this so that   * we can nullify the sigdeliver function pointer in the TCB and accept   * more signal deliveries while processing the current pending signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state */  irqrestore((uint16_t)regs[REG_PRIMASK]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sdbg("Resuming/n");  (void)irqsave();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of   * execution.   */  up_ledoff(LED_SIGNAL);  up_fullcontextrestore(regs);}

__EXPORT int nsh_archinitialize(void){	int result;	/* configure the high-resolution time/callout interface */	hrt_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);	board_pwr_init(1);	/* initial LED state */	drv_led_start();	led_off(LED_AMBER);	led_off(LED_BLUE);#if defined(FLASH_BASED_PARAMS)	static sector_descriptor_t  sector_map[] = {		{1, 16 * 1024, 0x08004000},		{2, 16 * 1024, 0x08008000},		{0, 0, 0},	};	/* Initalizee the flashfs layer to use heap allocated memory */	result = parameter_flashfs_init(sector_map, NULL, 0);	if (result != OK) {		message("[boot] FAILED to init params in FLASH %d/n", result);		up_ledon(LED_AMBER);		return -ENODEV;	}#endif	return OK;}

void up_allocate_heap(FAR void **heap_start, size_t *heap_size){#if defined(CONFIG_NUTTX_KERNEL) && defined(CONFIG_MM_KERNEL_HEAP)  /* Get the unaligned size and position of the user-space heap.   * This heap begins after the user-space .bss section at an offset   * of CONFIG_MM_KERNEL_HEAPSIZE (subject to alignment).   */  uintptr_t ubase = (uintptr_t)USERSPACE->us_bssend + CONFIG_MM_KERNEL_HEAPSIZE;  size_t    usize = SRAM1_END - ubase;  int       log2;  DEBUGASSERT(ubase < (uintptr_t)SRAM1_END);  /* Adjust that size to account for MPU alignment requirements.   * NOTE that there is an implicit assumption that the SRAM1_END   * is aligned to the MPU requirement.   */  log2  = (int)mpu_log2regionfloor(usize);  DEBUGASSERT((SRAM1_END & ((1 << log2) - 1)) == 0);  usize = (1 << log2);  ubase = SRAM1_END - usize;  /* Return the user-space heap settings */  up_ledon(LED_HEAPALLOCATE);  *heap_start = (FAR void*)ubase;  *heap_size  = usize;  /* Allow user-mode access to the user heap memory */   stm32_mpu_uheap((uintptr_t)ubase, usize);#else  /* Return the heap settings */  up_ledon(LED_HEAPALLOCATE);  *heap_start = (FAR void*)g_idle_topstack;  *heap_size  = SRAM1_END - g_idle_topstack;#endif}

void up_ledpminitialize(void){  /* Register to receive power management callbacks */  int ret = pm_register(&g_ledscb);  if (ret != OK)  {      up_ledon(LED_ASSERTION);    }}

uint32_t *up_doirq(int irq, uint32_t *regs){  up_ledon(LED_INIRQ);#ifdef CONFIG_SUPPRESS_INTERRUPTS  PANIC(OSERR_ERREXCEPTION);#else  uint32_t *savestate;  /* Nested interrupts are not supported in this implementation.  If you want   * to implement nested interrupts, you would have to (1) change the way that   * current_regs is handled and (2) the design associated with   * CONFIG_ARCH_INTERRUPTSTACK.  The savestate variable will not work for   * that purpose as implemented here because only the outermost nested   * interrupt can result in a context switch (it can probably be deleted).   */  /* Current regs non-zero indicates that we are processing an interrupt;   * current_regs is also used to manage interrupt level context switches.   */  savestate    = (uint32_t*)current_regs;  current_regs = regs;  /* Disable further occurences of this interrupt (until the interrupt sources   * have been clear by the driver.   */  up_disable_irq(irq);  /* Deliver the IRQ */  irq_dispatch(irq, regs);  /* If a context switch occurred while processing the interrupt then   * current_regs may have change value.  If we return any value different   * from the input regs, then the lower level will know that a context   * switch occurred during interrupt processing.   */  regs = (uint32_t*)current_regs;  /* Restore the previous value of current_regs.  NULL would indicate that   * we are no longer in an interrupt handler.  It will be non-NULL if we   * are returning from a nested interrupt.   */  current_regs = savestate;  /* Unmask the last interrupt (global interrupts are still disabled) */  up_enable_irq(irq);#endif  up_ledoff(LED_INIRQ);  return regs;}

uint32_t *up_doirq(int irq, uint32_t* regs){  up_ledon(LED_INIRQ);#ifdef CONFIG_SUPPRESS_INTERRUPTS  PANIC(OSERR_ERREXCEPTION);#else  if ((unsigned)irq < NR_IRQS)    {       uint32_t *savestate;       /* Current regs non-zero indicates that we are processing        * an interrupt; current_regs is also used to manage        * interrupt level context switches.        */       savestate    = (uint32_t*)current_regs;       current_regs = regs;       /* Mask and acknowledge the interrupt (if supported by the chip) */#ifndef CONFIG_ARCH_NOINTC       up_maskack_irq(irq);#endif       /* Deliver the IRQ */       irq_dispatch(irq, regs);       /* Get the current value of regs... it may have changed because        * of a context switch performed during interrupt processing.        */       regs = current_regs;       /* Restore the previous value of current_regs.  NULL would indicate that        * we are no longer in an interrupt handler.  It will be non-NULL if we        * are returning from a nested interrupt.        */       current_regs = savestate;       /* Unmask the last interrupt (global interrupts are still        * disabled.        */#ifndef CONFIG_ARCH_NOINTC       up_enable_irq(irq);#endif    }  up_ledoff(LED_INIRQ);#endif  return regs;}

int up_create_stack(_TCB *tcb, size_t stack_size){  if (tcb->stack_alloc_ptr &&      tcb->adj_stack_size != stack_size)    {      sched_free(tcb->stack_alloc_ptr);      tcb->stack_alloc_ptr = NULL;    }   if (!tcb->stack_alloc_ptr)     {#ifdef CONFIG_DEBUG       tcb->stack_alloc_ptr = (uint32_t*)kzalloc(stack_size);#else       tcb->stack_alloc_ptr = (uint32_t*)kmalloc(stack_size);#endif     }   if (tcb->stack_alloc_ptr)     {       size_t top_of_stack;       size_t size_of_stack;       /* MIPS uses a push-down stack:  the stack grows        * toward loweraddresses in memory.  The stack pointer        * register, points to the lowest, valid work address        * (the "top" of the stack).  Items on the stack are        * referenced as positive word offsets from sp.        */       top_of_stack = (uint32_t)tcb->stack_alloc_ptr + stack_size - 4;       /* The MIPS stack must be aligned at word (4 byte)        * boundaries. If necessary top_of_stack must be rounded        * down to the next boundary        */       top_of_stack &= ~3;       size_of_stack = top_of_stack - (uint32_t)tcb->stack_alloc_ptr + 4;       /* Save the adjusted stack values in the _TCB */       tcb->adj_stack_ptr  = (uint32_t*)top_of_stack;       tcb->adj_stack_size = size_of_stack;       up_ledon(LED_STACKCREATED);       return OK;     }   return ERROR;}

void up_allocate_heap(FAR void **heap_start, size_t *heap_size){    size_t size = CONFIG_DRAM_END - g_heapbase;    /* Return the heap settings */    up_ledon(LED_HEAPALLOCATE);    *heap_start = (FAR void*)g_heapbase;    *heap_size  = size;    /* Allow access to the heap memory */    sam3u_mpuheap((uintptr_)g_heapbase, size);}

void up_assert_code(const uint8_t *filename, int lineno, int errorcode){#if CONFIG_TASK_NAME_SIZE > 0 && defined(CONFIG_DEBUG)  _TCB *rtcb = (_TCB*)g_readytorun.head;#endif  up_ledon(LED_ASSERTION);#if CONFIG_TASK_NAME_SIZE > 0  lldbg("Assertion failed at file:%s line: %d task: %s error code: %d/n",        filename, lineno, rtcb->name, errorcode);#else  lldbg("Assertion failed at file:%s line: %d error code: %d/n",        filename, lineno, errorcode);#endif  up_dumpstate();  _up_assert(errorcode);}

void up_assert_code(const uint8_t *filename, int lineno, int errorcode){#ifdef CONFIG_PRINT_TASKNAME  struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;#endif  up_ledon(LED_ASSERTION);#ifdef CONFIG_PRINT_TASKNAME  lldbg("Assertion failed at file:%s line: %d task: %s error code: %d/n",        filename, lineno, rtcb->name, errorcode);#else  lldbg("Assertion failed at file:%s line: %d error code: %d/n",        filename, lineno, errorcode);#endif  up_dumpstate();  _up_assert(errorcode);}

uint8_t *up_doirq(int irq, uint8_t *regs){  up_ledon(LED_INIRQ);#ifdef CONFIG_SUPPRESS_INTERRUPTS  PANIC(OSERR_ERREXCEPTION);#else  uint8_t *savestate;  /* Nested interrupts are not supported in this implementation.  If you want   * implemented nested interrupts, you would have to (1) change the way that   * current regs is handled and (2) the design associated with   * CONFIG_ARCH_INTERRUPTSTACK.   */  /* Current regs non-zero indicates that we are processing an interrupt;   * current_regs is also used to manage interrupt level context switches.   */  savestate    = (uint8_t*)current_regs;  current_regs = regs;  /* Deliver the IRQ */  irq_dispatch(irq, regs);  /* If a context switch occurred while processing the interrupt then   * current_regs may have change value.  If we return any value different   * from the input regs, then the lower level will know that a context   * switch occurred during interrupt processing.   */  regs = (uint8_t*)current_regs;  /* Restore the previous value of current_regs.  NULL would indicate that   * we are no longer in an interrupt handler.  It will be non-NULL if we   * are returning from a nested interrupt.   */  current_regs = savestate;#endif  up_ledoff(LED_INIRQ);  return regs;}

static void _up_assert(int errorcode) /* __attribute__ ((noreturn)) */{  /* Are we in an interrupt handler or the idle task? */  if (up_interrupt_context() || ((FAR _TCB*)g_readytorun.head)->pid == 0)    {       (void)irqsave();        for(;;)          {#ifdef CONFIG_ARCH_LEDS            up_ledon(LED_PANIC);            up_mdelay(250);            up_ledoff(LED_PANIC);            up_mdelay(250);#endif          }    }  else    {      exit(errorcode);    }}

static void _up_assert(int errorcode){  /* Are we in an interrupt handler or the idle task? */  if (current_regs || ((struct tcb_s*)g_readytorun.head)->pid == 0)    {       (void)irqsave();        for(;;)          {#ifdef CONFIG_ARCH_LEDS            up_ledon(LED_PANIC);            up_mdelay(250);            up_ledoff(LED_PANIC);            up_mdelay(250);#endif          }    }  else    {      exit(errorcode);    }}

void up_idle(void){#if defined(CONFIG_ARCH_LEDS) && defined(CONFIG_ARCH_BRINGUP)  g_ledtoggle++;  if (g_ledtoggle == 0x80)    {      up_ledon(LED_IDLE);    }  else if (g_ledtoggle == 0x00)    {      up_ledoff(LED_IDLE);    }#endif#if defined(CONFIG_SUPPRESS_INTERRUPTS) || defined(CONFIG_SUPPRESS_TIMER_INTS)  /* If the system is idle and there are no timer interrupts,   * then process "fake" timer interrupts. Hopefully, something   * will wake up.   */  sched_process_timer();#endif}

FAR chipreg_t *up_doirq(uint8_t irq, FAR chipreg_t *regs){  up_ledon(LED_INIRQ);#ifdef CONFIG_SUPPRESS_INTERRUPTS  lowsyslog("Unexpected IRQ/n");  IRQ_ENTER(regs);  PANIC();  return NULL; /* Won't get here */#else  if (irq < NR_IRQS)    {       DECL_SAVESTATE();       /* Indicate that we have entered IRQ processing logic */       IRQ_ENTER(irq, regs);       /* Deliver the IRQ */       irq_dispatch(irq, regs);       /* If a context switch occurred, 'regs' will hold the new context */       regs = IRQ_STATE();       /* Indicate that we are no longer in interrupt processing logic */       IRQ_LEAVE(irq);    }  up_ledoff(LED_INIRQ);  return regs;#endif}

void up_allocate_heap(FAR void **heap_start, size_t *heap_size){    up_ledon(LED_HEAPALLOCATE);    *heap_start = (FAR void*)g_heapbase;    *heap_size  = SRAM1_END - g_heapbase;}

void up_allocate_heap(FAR void **heap_start, size_t *heap_size){  up_ledon(LED_HEAPALLOCATE);  *heap_start = (FAR void*)g_idle_topstack;  *heap_size  = (IMX_SDRAM_VSECTION + CONFIG_RAM_SIZE) - g_idle_topstack;}

void up_sigdeliver(void){  _TCB  *rtcb = (_TCB*)g_readytorun.head;#if 0  uint32_t regs[XCPTCONTEXT_REGS+3];  /* Why +3? See below */#else  uint32_t regs[XCPTCONTEXT_REGS];#endif  sig_deliver_t sigdeliver;  /* Save the errno.  This must be preserved throughout the signal handling   * so that the user code final gets the correct errno value (probably EINTR).   */  int saved_errno = rtcb->pterrno;  up_ledon(LED_SIGNAL);  sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p/n",        rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);  ASSERT(rtcb->xcp.sigdeliver != NULL);  /* Save the real return state on the stack. */  up_copystate(regs, rtcb->xcp.regs);  regs[REG_PC]         = rtcb->xcp.saved_pc;  regs[REG_SR]         = rtcb->xcp.saved_sr;  /* Get a local copy of the sigdeliver function pointer. We do this so that   * we can nullify the sigdeliver function pointer in the TCB and accept   * more signal deliveries while processing the current pending signals.   */  sigdeliver           = rtcb->xcp.sigdeliver;  rtcb->xcp.sigdeliver = NULL;  /* Then restore the task interrupt state */  irqrestore(regs[REG_SR]);  /* Deliver the signals */  sigdeliver(rtcb);  /* Output any debug messages BEFORE restoring errno (because they may   * alter errno), then disable interrupts again and restore the original   * errno that is needed by the user logic (it is probably EINTR).   */  sdbg("Resuming/n");  (void)irqsave();  rtcb->pterrno = saved_errno;  /* Then restore the correct state for this thread of execution. This is an   * unusual case that must be handled by up_fullcontextresore. This case is   * unusal in two ways:   *   *   1. It is not a context switch between threads.  Rather, up_fullcontextrestore   *      must behave more it more like a longjmp within the same task, using   *      he same stack.   *   2. In this case, up_fullcontextrestore is called with r12 pointing to   *      a register save area on the stack to be destroyed.  This is   *      dangerous because there is the very real possibility that the new   *      stack pointer might overlap with the register save area and hat stack   *      usage in up_fullcontextrestore might corrupt the register save data   *      before the state is restored.  At present, there does not appear to   *      be any stack overlap problems.  If there were, then adding 3 words   *      to the size of register save structure size will protect its contents.   */  up_ledoff(LED_SIGNAL);  up_fullcontextrestore(regs);}

void up_allocate_heap(FAR void **heap_start, size_t *heap_size){  *heap_start = (FAR void*)CONFIG_HEAP1_BASE;  *heap_size = CONFIG_HEAP1_END - CONFIG_HEAP1_BASE;  up_ledon(LED_HEAPALLOCATE);}

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;//.........这里部分代码省略.........

int up_create_stack(FAR struct tcb_s *tcb, size_t stack_size, uint8_t ttype){  /* Is there already a stack allocated of a different size?  Because of   * alignment issues, stack_size might erroneously appear to be of a   * different size.  Fortunately, this is not a critical operation.   */  if (tcb->stack_alloc_ptr && tcb->adj_stack_size != stack_size)    {      /* Yes.. Release the old stack */      up_release_stack(tcb, ttype);    }  /* Do we need to allocate a new stack? */   if (!tcb->stack_alloc_ptr)    {      /* Allocate the stack.  If DEBUG is enabled (but not stack debug),       * then create a zeroed stack to make stack dumps easier to trace.       */#if defined(CONFIG_NUTTX_KERNEL) && defined(CONFIG_MM_KERNEL_HEAP)      /* Use the kernel allocator if this is a kernel thread */      if (ttype == TCB_FLAG_TTYPE_KERNEL)        {#if defined(CONFIG_DEBUG) && !defined(CONFIG_DEBUG_STACK)          tcb->stack_alloc_ptr = (uint32_t *)kzalloc(stack_size);#else          tcb->stack_alloc_ptr = (uint32_t *)kmalloc(stack_size);#endif        }      else#endif        {          /* Use the user-space allocator if this is a task or pthread */#if defined(CONFIG_DEBUG) && !defined(CONFIG_DEBUG_STACK)          tcb->stack_alloc_ptr = (uint32_t *)kuzalloc(stack_size);#else          tcb->stack_alloc_ptr = (uint32_t *)kumalloc(stack_size);#endif        }#ifdef CONFIG_DEBUG      /* Was the allocation successful? */      if (!tcb->stack_alloc_ptr)        {          sdbg("ERROR: Failed to allocate stack, size %d/n", stack_size);        }#endif    }  /* Did we successfully allocate a stack? */  if (tcb->stack_alloc_ptr)    {      size_t top_of_stack;      /* Yes.. If stack debug is enabled, then fill the stack with a       * recognizable value that we can use later to test for high       * water marks.       */#if defined(CONFIG_DEBUG) && defined(CONFIG_DEBUG_STACK)      memset(tcb->stack_alloc_ptr, 0xaa, stack_size);#endif      /* The AVR uses a push-down stack:  the stack grows toward lower       * addresses in memory.  The stack pointer register, points to the       * lowest, valid work address (the "top" of the stack).  Items on the       * stack are referenced as positive word offsets from sp.       */      top_of_stack = (size_t)tcb->stack_alloc_ptr + stack_size - 1;      /* Save the adjusted stack values in the struct tcb_s */      tcb->adj_stack_ptr  = (FAR void *)top_of_stack;      tcb->adj_stack_size = stack_size;      up_ledon(LED_STACKCREATED);      return OK;    }   return ERROR;}

__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;	}//.........这里部分代码省略.........

uint32_t *pic32mx_decodeirq(uint32_t *regs){#ifdef CONFIG_SUPPRESS_INTERRUPTS  up_ledon(LED_INIRQ);  PANIC(OSERR_ERREXCEPTION);  up_ledoff(LED_INIRQ); /* Won't get here */  return regs;#else  uint32_t *savestate;  uint32_t regval;  int irq;  /* If the board supports LEDs, turn on an LED now to indicate that we are   * processing an interrupt.   */  up_ledon(LED_INIRQ);  /* Save the current value of current_regs (to support nested interrupt   * handling).  Then set current_regs to regs, indicating that this is   * the interrupted context that is being processed now.   */  savestate    = (uint32_t*)current_regs;  current_regs = regs;  /* Loop while there are pending interrupts with priority greater than zero */  for (;;)    {      /* Read the INTSTAT register.  This register contains both the priority       * and the interrupt vector number.       */      regval = getreg32(PIC32MX_INT_INTSTAT);      if ((regval & INT_INTSTAT_RIPL_MASK) == 0)        {          /* Break out of the loop when the priority is zero meaning that           * there are no further pending interrupts.           */          break;        }      /* Get the vector number.  The IRQ numbers have been arranged so that       * vector numbers and NuttX IRQ numbers are the same value.       */      irq = ((regval) & INT_INTSTAT_VEC_MASK) >> INT_INTSTAT_VEC_SHIFT;            /* Disable further interrupts from this source until the driver has       * cleared the pending interrupt sources.       */      up_disable_irq(irq);      /* Deliver the IRQ */      irq_dispatch(irq, regs);      /* Unmask the last interrupt (global interrupt below the current interrupt       * level are are still disabled)       */      up_enable_irq(irq);    }  /* If a context switch occurred while processing the interrupt then   * current_regs may have change value.  If we return any value different   * from the input regs, then the lower level will know that a context   * switch occurred during interrupt processing.   */  regs = (uint32_t*)current_regs;  /* Restore the previous value of current_regs.  NULL would indicate that   * we are no longer in an interrupt handler.  It will be non-NULL if we   * are returning from a nested interrupt.   */  current_regs = savestate;  if (current_regs == NULL)    {      up_ledoff(LED_INIRQ);    }  return regs;#endif}

__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_IN10);	/* used by VBUS valid */	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */	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);	/* configure the high-resolution time/callout interface */	hrt_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_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);	message("[boot] Successfully initialized SPI port 1/n");	/* 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;	}	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */	SPI_SETFREQUENCY(spi2, 375000000);	SPI_SETBITS(spi2, 8);	SPI_SETMODE(spi2, SPIDEV_MODE3);	SPI_SELECT(spi2, SPIDEV_FLASH, false);	message("[boot] Successfully initialized SPI port 2/n");	#ifdef CONFIG_MMCSD	/* First, get an instance of the SDIO interface */	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);	if (!sdio) {		message("nsh_archinitialize: Failed to initialize SDIO slot %d/n",			CONFIG_NSH_MMCSDSLOTNO);		return -ENODEV;	}//.........这里部分代码省略.........