基于pixhawk2.4.6硬件和ChibiOS系统的ardupilot启动流程:从上电到ArduCopter应用层代码

NOTE: 以 —>>> 开头的为加入的分析和注解 <<<—

链接脚本common.ld

./libraries/AP_HAL_ChibiOS/hwdef/common/common.ld

/*
    this file is included by the board specific ldscript.ld which is
    generated from hwdef.dat 
    --->>>  该文件被从hwdef.dat 生成的具体板子相关的ldscript.ld 文件包含
    例如编译完成后: 
    build/fmuv2/ldscript.ld 如下
    关于MEMORY:
    The MEMORY command describes the location and size of blocks of memory in the target. 
    You can use it to describe which memory regions may be used by the linker, and which 
    memory regions it must avoid. You can then assign sections to particular memory regions. 
    The linker will set section addresses based on the memory regions, and will warn about 
    regions that become too full. The linker will not shuffle sections around to fit into the available regions. <<<---
    /* generated ldscript.ld */ --->>> 指定ROM和RAM的起始地址origin和长度length <<<---
    MEMORY
    {
        flash : org = 0x08004000, len = 1008K
        ram0  : org = 0x20000000, len = 192k
    }

   INCLUDE common.ld --->>> 这里包含了我们的common.ld  <<<---
   
   ---> build/iomcu/ldscript.ld
   /* generated ldscript.ld */ --->>> 指定ROM和RAM的起始地址和长度  <<<---
   MEMORY
   {
       flash : org = 0x08001000, len = 60K
       ram0  : org = 0x20000000, len = 8k
   }

   INCLUDE common.ld ---> >> 这里包含了我们的common.ld  <<<---
    
*/
--->>> 关于REGION_ALIAS: The REGION_ALIAS function creates an alias name alias for the 
memory region region. This allows a flexible mapping of output sections to memory regions.  <<<---
/* RAM region to be used for Main stack. This stack accommodates the processing
   of all exceptions and interrupts --->>> 中断和异常处理栈 <<<---*/
REGION_ALIAS("MAIN_STACK_RAM", ram0);

/* RAM region to be used for the process stack. This is the stack used by
   the main() function. --->>> 主函数进程栈的ram使用ram0 <<<---*/
REGION_ALIAS("PROCESS_STACK_RAM", ram0);

/* RAM region to be used for data segment. --->>> 数据段ram使用ram0  <<<---*/
REGION_ALIAS("DATA_RAM", ram0);

/* RAM region to be used for BSS segment. --->>> BSS段ram使用ram0 <<<---*/
REGION_ALIAS("BSS_RAM", ram0);

/* RAM region to be used for the default heap.--->>> 堆ram使用ram0 <<<---*/
REGION_ALIAS("HEAP_RAM", ram0);

__ram0_start__          = ORIGIN(ram0); --->>> 符号定义,ram0的起始地址 <<<---
__ram0_size__           = LENGTH(ram0); --->>> 符号定义,ram0的长度  <<<---
__ram0_end__            = __ram0_start__ + __ram0_size__; --->>> 符号定义,ram0的结束地址 <<<---

--->>> 关于ENTRY: The first instruction to execute in a program is called the entry point. You 
can use the ENTRY linker script command to set the entry point. The argument is a symbol name: ENTRY(symbol) <<<---

ENTRY(Reset_Handler) --->>> 定义入口地址标号 在后面中断向量vector.S中分析  <<<---

SECTIONS
{
    . = 0;
    _text = .;                      --->>> 符号定义 <<<---
--->>> STARTUP 解释
The STARTUP command is just like the INPUT command, except that filename will become the 
first input file to be linked, as though(就像) it were specified first on the command line. This may be 
useful when using a system in which the entry point is always the start of the first file. <<<---
    startup : ALIGN(16) SUBALIGN(16) 
    {
        KEEP(*(.vectors))
    } > flash --->>> 关于" > "的解释: Once you define a memory region, you can direct the linker to place specific output sections into that memory region by using the ‘>region’ output section attribute. <<<---

    constructors : ALIGN(4) SUBALIGN(4) --->>> 构造函数段 <<<---
    {
        __init_array_start = .; --->>>  定义符号表征开始地址 <<<---
        --->>> 关于KEEP: When link-time garbage collection is in use (-gc-sections), it is often useful 
        to mark sections that should not be eliminated. This is accomplished by surrounding an 
        input section's wildcard entry with KEEP(), as in KEEP(*(.init)) or KEEP(SORT(*)(.ctors)). <<<---
        --->>> 关于SORT: SORT is an alias for SORT_BY_NAME. Normally, the linker will place 
files and sections matched by wildcards in the order in which they are seen during the link. You can change this by using the SORT_BY_NAME keyword, which appears before a wildcard pattern in parentheses (e.g., SORT_BY_NAME(.text*)). When the SORT_BY_NAME keyword 
is used, the linker will sort the files or sections into ascending order by name before placing 
them in the output file. <<<---
        KEEP(*(SORT(.init_array.*)))
        KEEP(*(.init_array))
        __init_array_end = .;  --->>>  定义符号表征结束地址 <<<---
    } > flash

    destructors : ALIGN(4) SUBALIGN(4)
    {
        __fini_array_start = .;  --->>>  定义符号表征开始地址 <<<---
        KEEP(*(.fini_array))
        KEEP(*(SORT(.fini_array.*)))
        __fini_array_end = .;  --->>>  定义符号表征结束地址 <<<---
    } > flash

    .text : ALIGN(4) SUBALIGN(4) --->> 代码段和只读数据段 <<<---
    {
        *(.text)
        *(.text.*)
        *(.rodata)
        *(.rodata.*)
        *(.glue_7t)
        *(.glue_7)
        *(.gcc*)
    } > flash

    .ARM.extab :
    {
        *(.ARM.extab* .gnu.linkonce.armextab.*)
    } > flash

    .ARM.exidx : {
        __exidx_start = .;
        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
        __exidx_end = .;
     } > flash

    .eh_frame_hdr :
    {
        *(.eh_frame_hdr)
    } > flash

	--->>> ONLY_IF_RO 关键字用于表示在所有输入文件的 section 为只读的时候,输出 section 才能生成 <<<---
    .eh_frame : ONLY_IF_RO
    {
        *(.eh_frame)
    } > flash

    .textalign : ONLY_IF_RO
    {
        . = ALIGN(8);
    } > flash

    /* Legacy symbol, not used anywhere.*/
    . = ALIGN(4);
    ---> 关于PROVIDE: In this example, if the program defines ‘_etext’ (with a leading underscore),
     the linker will give a multiple definition error. If, on the other hand, the program defines ‘etext’ 
     (with no leading underscore), the linker will silently use the definition in the program. If the
      program references ‘etext’ but does not define it, the linker will use the definition in the linker script.
    PROVIDE(_etext = .);

    /* Special section for exceptions stack.*/
    .mstack :
    {
        . = ALIGN(8);
        __main_stack_base__ = .;  --->>> 定义符号 <<<---
        . += __main_stack_size__; --->>> 编译时定义符号: 通过在命令行使用--defsym symbol=expression 指示编译器 ,在makefile中定义
        . = ALIGN(8);
        __main_stack_end__ = .; --->>> 定义符号 <<<---
    } > MAIN_STACK_RAM

    /* Special section for process stack.*/
    .pstack :
    {
        __process_stack_base__ = .; --->>>  定义符号,进程栈基址 <<<---
        __main_thread_stack_base__ = .; --->>> 定义符号,主线程栈基址
        . += __process_stack_size__; --->>> 编译时定义符号: 通过在命令行使用--defsym symbol=expression 指示编译器 
        Create a global symbol in the output file, containing the absolute address given by 
        expression. You may use this option as many times as necessary to define multiple
         symbols in the command line. 在makefile中定义如: --defsym=__process_stack_size__=0x2000
        . = ALIGN(8);
        __process_stack_end__ = .;
        __main_thread_stack_end__ = .;
    } > PROCESS_STACK_RAM

    .data : ALIGN(4) --->>> 读写数据段 <<<---
    {
        . = ALIGN(4);
        --->>> 关于LOADADDR: Return the absolute load address of the named section. This is normally the same as ADDR, but it may be different if the AT keyword is used in the section definition <<<---
        PROVIDE(_textdata = LOADADDR(.data));
        PROVIDE(_data = .);
        _textdata_start = LOADADDR(.data);
        _data_start = .;
        *(.data)
        *(.data.*)
        *(.ramtext)
        . = ALIGN(4);
        PROVIDE(_edata = .);
        _data_end = .;
         --->>> 关于AT: The expression ldadr that follows the AT keyword specifies the 
        load address of the section. The default (if you do not use the AT keyword) is to make the 
        load address the same as the relocation address. This feature is designed to make it easy 
        to build a ROM image. <<<---
    } > DATA_RAM AT > flash

    .bss (NOLOAD) : ALIGN(4) --->>> BSS段
    {
        . = ALIGN(4);
        _bss_start = .;
        *(.bss)
        *(.bss.*)
        *(COMMON) --->>> COMMON: A special notation is needed for common symbols, 
        because in many object file formats common symbols do not have a particular input 
        section. The linker treats common symbols as though they are in an input section named ‘COMMON’. ---<<< 
        . = ALIGN(4);
        _bss_end = .;
        PROVIDE(end = .);
    } > BSS_RAM

    .ram0_init : ALIGN(4)
    {
        . = ALIGN(4);
        __ram0_init_text__ = LOADADDR(.ram0_init);
        __ram0_init__ = .;
        *(.ram0_init)
        *(.ram0_init.*)
        . = ALIGN(4);
    } > ram0 AT > flash

	--->>> NOLOAD: The `(NOLOAD)' directive will mark a section to not be loaded at run time. 
	The linker will process the section normally, but will mark it so that a program loader will not
	 load it into memory.<<<---
    .ram0 (NOLOAD) : ALIGN(4)
    {
        . = ALIGN(4);
        __ram0_clear__ = .;
        *(.ram0_clear)
        *(.ram0_clear.*)A
        . = ALIGN(4);
        __ram0_noinit__ = .;
        *(.ram0)
        *(.ram0.*)
        . = ALIGN(4);
        __ram0_free__ = .;
    } > ram0

    /* The default heap uses the (statically) unused part of a RAM section.*/
    .heap (NOLOAD) : --->>> 堆 
    {
        . = ALIGN(8);
        __heap_base__ = .;
        . = ORIGIN(HEAP_RAM) + LENGTH(HEAP_RAM);
        __heap_end__ = .;
    } > HEAP_RAM
}

1. 中断向量表vectors.S

ardupilot\modules\ChibiOS\os\common\startup\ARMCMx\compilers\GCC下:

        .globl      _vectors
_vectors:
        .long       __main_stack_end__    --->>> 该符号在链接脚本中有定义 
        .long       Reset_Handler         --->>> 上电的入口,即链接脚本中: ENTRY(Reset_Handler) 
        .long       NMI_Handler
        .long       HardFault_Handler
        .long       MemManage_Handler
        .long       BusFault_Handler
        .long       UsageFault_Handler
        .long       Vector1C
        .long       Vector20
        .long       Vector24
        .long       Vector28
        .long       SVC_Handler
        .long       DebugMon_Handler
        .long       Vector34
        .long       PendSV_Handler
        .long       SysTick_Handler

2. 程序入口Reset_Handler

        .text

        .align      2
        .thumb_func
        .weak       Reset_Handler ---> 这里定义为弱符号,可以使用强定义符号进行覆盖
Reset_Handler:
         b          _crt0_entry         ---> 跳转到了_crt0_entry

3. 追踪_crt0_entry

crt0_v7m.S ardupilot\modules\ChibiOS\os\common\startup\ARMCMx\compilers\GCC下

/*
 * CRT0 entry point.
 */
                .align  2
                .thumb_func
                .global _crt0_entry
_crt0_entry:                                                    --->>> _crt0_entry入口定义
                /* Interrupts are globally masked initially.*/  --->>>  先全局屏蔽掉中断
                cpsid   i

#if CRT0_FORCE_MSP_INIT == TRUE
                /* MSP stack pointers initialization.*/ --->>>  msp 栈指针初始化
                ldr     r0, =__main_stack_end__       --->>>  该符号在链接脚本中有定义,向下增长,所以使用end
                msr     MSP, r0
#endif

                /* PSP stack pointers initialization.*/ --->>>  psp 栈指针初始化
                ldr     r0, =__process_stack_end__ --->>>  该符号在链接脚本中有定义
                msr     PSP, r0

#if CRT0_VTOR_INIT == TRUE --->>>  中断向量表写入高端地址
                ldr     r0, =_vectors                         --->>>  获取符号_vectors表征的地址
                movw    r1, #SCB_VTOR & 0xFFFF
                movt    r1, #SCB_VTOR >> 16
                str     r0, [r1]                                  --->>>  _vctors 存储到高地址
#endif

#if CRT0_INIT_FPU == TRUE --->>> 浮点运算相关设置
                /* FPU FPCCR initialization.*/
                movw    r0, #CRT0_FPCCR_INIT & 0xFFFF
                movt    r0, #CRT0_FPCCR_INIT >> 16
                movw    r1, #SCB_FPCCR & 0xFFFF
                movt    r1, #SCB_FPCCR >> 16
                str     r0, [r1]
                dsb
                isb

                /* CPACR initialization.*/ --->>> 协处理器初始化
                movw    r0, #CRT0_CPACR_INIT & 0xFFFF
                movt    r0, #CRT0_CPACR_INIT >> 16
                movw    r1, #SCB_CPACR & 0xFFFF
                movt    r1, #SCB_CPACR >> 16
                str     r0, [r1]
                dsb
                isb

                /* FPU FPSCR initially cleared.*/
                mov     r0, #0
                vmsr    FPSCR, r0

                /* FPU FPDSCR initially cleared.*/
                movw    r1, #SCB_FPDSCR & 0xFFFF
                movt    r1, #SCB_FPDSCR >> 16
                str     r0, [r1]

                /* Enforcing FPCA bit in the CONTROL register.*/
                movs    r0, #CRT0_CONTROL_INIT | CONTROL_FPCA

#else
                movs    r0, #CRT0_CONTROL_INIT
#endif

                /* CONTROL register initialization as configured.*/
                msr     CONTROL, r0
                isb

#if CRT0_INIT_CORE == TRUE
                /* Core initialization.*/ --->__core_init 核心初始化 4.分析
                bl      __core_init
#endif

                /* Early initialization.*/ ---> __early_init 先期初始化 5. 分析
                bl      __early_init

#if CRT0_INIT_STACKS == TRUE
                ldr     r0, =CRT0_STACKS_FILL_PATTERN
                /* Main Stack initialization. Note, it assumes that the
                   stack size is a multiple of 4 so the linker file must
                   ensure this.*/ ---> 主栈初始化
                ldr     r1, =__main_stack_base__ --->>> 使用链接脚本中定义的符号
                ldr     r2, =__main_stack_end__ --->>> 使用链接脚本中定义的符号
msloop:
                cmp     r1, r2
                itt     lo
                strlo   r0, [r1], #4
                blo     msloop
                
                /* Process Stack initialization. Note, it assumes that the
                   stack size is a multiple of 4 so the linker file must
                   ensure this.*/ ---> 进程栈初始化
                ldr     r1, =__process_stack_base__        --->>> 使用链接脚本中定义的符号
                ldr     r2, =__process_stack_end__        --->>> 使用链接脚本中定义的符号
psloop:
                cmp     r1, r2
                itt     lo
                strlo   r0, [r1], #4
                blo     psloop
#endif

#if CRT0_INIT_DATA == TRUE
                /* Data initialization. Note, it assumes that the DATA size
                  is a multiple of 4 so the linker file must ensure this.*/
                   --->>> 数据段初始化 将数据段从加载地址,拷贝到数据段运行地址
                ldr     r1, =_textdata_start 
                ldr     r2, =_data_start
                ldr     r3, =_data_end
dloop:
                cmp     r2, r3
                ittt    lo
                ldrlo   r0, [r1], #4
                strlo   r0, [r2], #4
                blo     dloop
#endif

#if CRT0_INIT_BSS == TRUE
                /* BSS initialization. Note, it assumes that the DATA size 
                  is a multiple of 4 so the linker file must ensure this.*/ --->>> BSS段清零
                movs    r0, #0
                ldr     r1, =_bss_start
                ldr     r2, =_bss_end
bloop:
                cmp     r1, r2
                itt     lo
                strlo   r0, [r1], #4
                blo     bloop
#endif

#if CRT0_INIT_RAM_AREAS == TRUE 
                /* RAM areas initialization.*/ ---> RAM区域初始化
                bl      __init_ram_areas
#endif

                /* Late initialization..*/ ---> 后期初始化
                bl      __late_init

#if CRT0_CALL_CONSTRUCTORS == TRUE
                /* Constructors invocation.*/ --->>> 构造器调用,先于main进行构造
                ldr     r4, =__init_array_start --->>> 加载__init_array_start符号代表的地址值到寄存器
                ldr     r5, =__init_array_end
initloop:
                cmp     r4, r5
                bge     endinitloop
                ldr     r1, [r4], #4  --->>> 相当于取该符号代表的地址中的数据*(__init_array_start)
                blx     r1              --->>> 跳转到获得的数据中,即构造函数调用
                b       initloop
endinitloop:
#endif

                /* Main program invocation, r0 contains the returned value.*/ --->>> main 函数 6.分析
                bl      main --->>> 9.分析

#if CRT0_CALL_DESTRUCTORS == TRUE
                /* Destructors invocation.*/ ---> 析构器调用,main退出后进行析构
                ldr     r4, =__fini_array_start
                ldr     r5, =__fini_array_end
finiloop:
                cmp     r4, r5
                bge     endfiniloop
                ldr     r1, [r4], #4
                blx     r1
                b       finiloop
endfiniloop:
#endif
                /* Branching to the defined exit handler.*/ ---> 最后是默认退出handler
                b       __default_exit 
        位于:modules/ChibiOS/os/common/startup/ARMCAx-TZ/compilers/GCC/crt1.c        
        __attribute__((noreturn, weak)) 定
义为弱符号 
        /*lint -save -e9075 [8.4] All symbols are invoked from asm context.*/
        void __default_exit(void) {
        /*lint -restore*/
            while (true) {
            }
        }       

4. __core_init 核心初始化

modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/crt1.c:

/**
 * @brief   Architecture-dependent core initialization.
 * @details This hook is invoked immediately after the stack initialization
 *          and before the DATA and BSS segments initialization.
 * @note    This function is a weak symbol. --->>> 依然是弱符号,栈初始化完成后立即调用
 */
__attribute__((weak))
/*lint -save -e9075 [8.4] All symbols are invoked from asm context.*/
void __core_init(void) {
#if CORTEX_MODEL == 7
  SCB_EnableICache();
  SCB_EnableDCache();
#endif
}

5. __early_init 前期初始化

modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/crt1.c中的弱符号被libraries/AP_HAL_ChibiOS/hwdef/common/board.c中的强符号覆盖

/**
 * @brief   Early initialization code.
 * @details This initialization must be performed just after stack setup
 *          and before any other initialization. --->>> 栈就绪之后,其他初始化之前
 */
void __early_init(void) { --->>> gpio和系统时钟
#ifndef STM32F100_MCUCONF
  stm32_gpio_init();
#endif
  stm32_clock_init();
#if defined(HAL_DISABLE_DCACHE)
  SCB_DisableDCache();
#endif
}

6. __late_init 后期初始化

modules/ChibiOS/os/common/startup/ARMCMx/compilers/GCC/crt1.c 中的弱符号被 libraries/AP_HAL_ChibiOS/hwdef/common/board.c 中的强符号覆盖

void __late_init(void) {
  halInit(); --->>> HAL层 初始化 7. 分析
  chSysInit(); --->>> chibios系统初始化 8. 分析
  stm32_watchdog_save_reason();
#ifndef HAL_BOOTLOADER_BUILD
  stm32_watchdog_clear_reason();
#endif
#if CH_CFG_USE_HEAP == TRUE --->>> 堆初始化
  malloc_init();
#endif
#ifdef HAL_USB_PRODUCT_ID
  setup_usb_strings();
#endif
}

7. halInit(): HAL层初始化

/**
 * @brief   HAL initialization.
 * @details This function invokes the low level initialization code then
 *          initializes all the drivers enabled in the HAL. Finally the
 *          board-specific initialization is performed by invoking
 *          @p boardInit() (usually defined in @p board.c).
 * --->>> 调用底层初始化代码(lld),初始化HAL层的所有驱动外设,最后是板子特定的初始化
 * @init
 */
void halInit(void) {

  /* Initializes the OS Abstraction Layer.*/ --->>> 操作系统抽象层初始化
  osalInit();

  /* Platform low level initializations.*/ --->>> 平台底层初始化
  hal_lld_init();

#if (HAL_USE_PAL == TRUE) || defined(__DOXYGEN__)
#if defined(PAL_NEW_INIT)
  palInit();                              --->>> 平台抽象层初始化
#else
  palInit(&pal_default_config);
#endif
#endif
#if (HAL_USE_ADC == TRUE) || defined(__DOXYGEN__)
  adcInit();
#endif
#if (HAL_USE_CAN == TRUE) || defined(__DOXYGEN__)
  canInit();
#endif
#if (HAL_USE_CRY == TRUE) || defined(__DOXYGEN__) --->>> 加密
  cryInit();
#endif
#if (HAL_USE_DAC == TRUE) || defined(__DOXYGEN__)
  dacInit();
#endif
#if (HAL_USE_GPT == TRUE) || defined(__DOXYGEN__) --->>> 通用定时器
  gptInit();
#endif
#if (HAL_USE_I2C == TRUE) || defined(__DOXYGEN__)
  i2cInit();
#endif
#if (HAL_USE_I2S == TRUE) || defined(__DOXYGEN__)
  i2sInit();
#endif
#if (HAL_USE_ICU == TRUE) || defined(__DOXYGEN__)
  icuInit();
#endif
#if (HAL_USE_EICU == TRUE) || defined(__DOXYGEN__)
  eicuInit();
#endif
#if (HAL_USE_MAC == TRUE) || defined(__DOXYGEN__)
  macInit();
#endif
#if (HAL_USE_PWM == TRUE) || defined(__DOXYGEN__)
  pwmInit();
#endif
#if (HAL_USE_SERIAL == TRUE) || defined(__DOXYGEN__)
  sdInit();
#endif
#if (HAL_USE_SDC == TRUE) || defined(__DOXYGEN__)
  sdcInit();
#endif
#if (HAL_USE_SPI == TRUE) || defined(__DOXYGEN__)
  spiInit();
#endif
#if (HAL_USE_TRNG == TRUE) || defined(__DOXYGEN__)
  trngInit();
#endif
#if (HAL_USE_UART == TRUE) || defined(__DOXYGEN__)
  uartInit();
#endif
#if (HAL_USE_USB == TRUE) || defined(__DOXYGEN__)
  usbInit();
#endif
#if (HAL_USE_MMC_SPI == TRUE) || defined(__DOXYGEN__)
  mmcInit();
#endif
#if (HAL_USE_SERIAL_USB == TRUE) || defined(__DOXYGEN__)
  sduInit();
#endif
#if (HAL_USE_RTC == TRUE) || defined(__DOXYGEN__)
  rtcInit();
#endif
#if (HAL_USE_WDG == TRUE) || defined(__DOXYGEN__)
  wdgInit();
#endif
#if (HAL_USE_WSPI == TRUE) || defined(__DOXYGEN__)
  wspiInit();
#endif

  /* Community driver overlay initialization.*/ --->>> 未定义
#if defined(HAL_USE_COMMUNITY) || defined(__DOXYGEN__)
#if (HAL_USE_COMMUNITY == TRUE) || defined(__DOXYGEN__)
  halCommunityInit();
#endif
#endif

  /* Board specific initialization.*/ --->>> 板子相关的初始化 libraries/AP_HAL_ChibiOS/hwdef/common/board.c
  boardInit();

/*
 *  The ST driver is a special case, it is only initialized if the OSAL is
 *  configured to require it.
 */                                    --->>> ST相关
#if OSAL_ST_MODE != OSAL_ST_MODE_NONE
  stInit();
#endif
}

8. chSysInit(): chibios系统初始化

modules\ChibiOS\os\rt\src\chsys.c

/**
 * @brief   ChibiOS/RT initialization.
 * @details After executing this function the current instructions stream
 *          becomes the main thread.
 * @pre     Interrupts must disabled before invoking this function.
 * @post    The main thread is created with priority @p NORMALPRIO and
 *          interrupts are enabled.
 *
 * @special --->>> 执行完该函数后,当前指令流变成主线程
 */
void chSysInit(void) {

  _scheduler_init(); --->>> 调度器
  _vt_init();    --->>> 虚拟定时器
  _trace_init();

#if CH_DBG_SYSTEM_STATE_CHECK == TRUE
  ch.dbg.isr_cnt  = (cnt_t)0;
  ch.dbg.lock_cnt = (cnt_t)0;
#endif
#if CH_CFG_USE_TM == TRUE
  _tm_init();
#endif
#if CH_CFG_USE_MEMCORE == TRUE
  _core_init();
#endif
#if CH_CFG_USE_HEAP == TRUE
  _heap_init();
#endif
#if CH_CFG_USE_FACTORY == TRUE
  _factory_init();
#endif
#if CH_DBG_STATISTICS == TRUE
  _stats_init();
#endif

#if CH_CFG_NO_IDLE_THREAD == FALSE
  /* Now this instructions flow becomes the main thread.*/
#if CH_CFG_USE_REGISTRY == TRUE
  currp = _thread_init(&ch.mainthread, (const char *)&ch_debug, NORMALPRIO);
#else
  currp = _thread_init(&ch.mainthread, "main", NORMALPRIO);
#endif
#else
  /* Now this instructions flow becomes the idle thread.*/
  currp = _thread_init(&ch.mainthread, "idle", IDLEPRIO);
#endif

#if CH_DBG_ENABLE_STACK_CHECK == TRUE --->>> 栈检查 
  {
    /* Setting up the base address of the static main thread stack, the
       symbol must be provided externally.*/
     --->>> 符号 __main_thread_stack_base__ 在前述链接脚本中定义
    extern stkalign_t __main_thread_stack_base__;
    currp->wabase = &__main_thread_stack_base__;
  }
#elif CH_CFG_USE_DYNAMIC == TRUE
  currp->wabase = NULL;
#endif

  /* Setting up the caller as current thread.*/ --->>> 设置调用者为当前线程
  currp->state = CH_STATE_CURRENT;

  /* Port layer initialization last because it depend on some of the
     initializations performed before.*/ --->>> 移植相关的初始化
  port_init();

#if CH_DBG_STATISTICS == TRUE
  /* Starting measurement for this thread.*/
  chTMStartMeasurementX(&currp->stats);
#endif

  /* Initialization hook.*/
  CH_CFG_SYSTEM_INIT_HOOK();

  /* It is alive now.*/
  chSysEnable();

#if CH_CFG_NO_IDLE_THREAD == FALSE --->>> 有空闲线程
  {
    static const thread_descriptor_t idle_descriptor = {
      "idle",
      THD_WORKING_AREA_BASE(ch_idle_thread_wa),
      THD_WORKING_AREA_END(ch_idle_thread_wa),
      IDLEPRIO,
      _idle_thread,
      NULL
    };

    /* This thread has the lowest priority in the system, its role is just to
       serve interrupts in its context while keeping the lowest energy saving
       mode compatible with the system status.*/
    (void) chThdCreate(&idle_descriptor); --->>> 创建 idle 线程
  }
#endif
}

到此整个后期初始化完成,底层驱动和操作系统均已就绪,操作系统进入idle线程,开始应用层展开业务逻辑,下面我们看main函数如何展开应用层业务逻辑的: main之前先调用构造函数阵列,也就是循环取函数地址,然后跳转调用


9. bl main: 展开应用层业务逻辑

libraries\AP_HAL\AP_HAL_Main.h:

#include "HAL.h"

#ifndef AP_MAIN
#define AP_MAIN main
#endif

#define AP_HAL_MAIN() \
    AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
    extern "C" {                               \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, &callbacks); \
        return 0; \
    } \
    }

#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, CALLBACKS); \
        return 0; \
    } \
    }

main的具体实现框架参考:
AP_HAL 再分析, 以pixhawk-fmuv2为硬件平台,ChibiOS为底层操作系统:

总结:

  1. 链接脚本以section的形式决定可执行文件的ROM和RAM layout,以及入口。代码可以引用定义在链接脚本中的符号,反之亦然,也可以使用命令行参数传递定义的符号;
  2. 上电后,程序执行入口指令Reset_Handler,跳转到crt0 entry,屏蔽掉中断,初始化堆栈,重定位中断向量表,初始化协处理器,浮点处理器等;
  3. 跳转到核心初始化,使能指令/数据cache;
  4. 跳转到前期初始化,主要初始化gpio和系统时钟;
  5. 跳转到后期初始化,主要包括,HAL层,ChibiOS系统初始化化启动idle线程,malloc相关堆初始化,usb设置等;
  6. 此时操作系统和底层硬件均已就绪,跳转到main,展开对应的四大平台的业务逻辑;

你可能感兴趣的:(Ardupilot,源码剖析)