PX4_Bootloader分析

1 引言

半年前入手了Pixhawk V2全套硬件,编译好的开源固件也下了,四轴也飞了,一直想对这套开源飞控进行一个系统地解析,由于工作原因一直没时间。最近翻开了PX4飞控源代码,它基于NUTTX操作系统,在github上更新十分迅速。为了能够全面地掌握这套软硬件设计思想,同时对硬件系统有全面的认识,我决定对PX4 Bootloader进行详细解析。凡涉及到硬件相关的部分,本文以Pixhawk V2的主控STM32F427和IO协处理器STM32F100为基础进行解析,其他硬件可参照此方法进行类比,基本结构都是相似的。

感谢韦东山老师u-boot视频,我用同样的方法对PX4 Bootloader采用类似的方法进行了分析。针对我解析中存在的问题,希望同行和前辈们能够不吝赐教,谢谢。

2 PX4 Bootloader系统架构

从github上下载之后,打开Bootloader文件夹,可以看到libopencm3和Tools共2个文件夹和一共30个文件。

libopencm3和Tools这两个文件夹的主要功能如下:

  • Tools文件夹主要用于coding style和git submodule的检查,不需要太注意。
  • libopencm3是github上针对STM32系列芯片的开源库,Bootloader使用其中的库函数对相应型号的芯片进行操作,用到的地方将对库函数功能作简要说明,这里并不展开讨论。

其余30个文件主要功能如下:

  • 工程文件:包括Bootloader.sublime-project共1个文件,为sublime编辑器的工程文件。
  • Readme:包括Readme.md文件共1个,自己看,都能看懂。
  • License:包括LISENCE.md文件1个,权限文件,自己看。
  • Makefile文件:包括Makefile、Makefile.f1、Makefile.f2、Makefile.f3、Makefile.f4、Makefile.f7共5个文件,为系统的编译过程提供了依据。
  • Jtag配置文件:以.cfg结尾,包括jig_px4fmu.cfg、stm32f1x.cfg、stm32f3x.cfg、stm32f4x.cfg、stm32f102.cfg共5个文件,分别对应不同的Jtag烧写器、不同芯片使用openocd进行烧写时的配置。根据原理图可知,PX4 Bootloader可以使用Jtag接口进行程序烧写,某宝上买的Pixhawk V2拆开后在板上可以看到两个Jtag接口(分别对应主控STM32F427和协处理STM32F100),但是没有焊接出来,如需烧写bootloader程序可自行焊接。
  • 硬件配置头函数:包括hw_config.h共1个文件,规定了各类可运行PX4开源飞控程序的硬件配置,可与原理图对应查看。
  • 通用bootloader调用函数集:包括bl.h和bl.c共2个文件。提供了bootloader的main函数中需要调用的通用函数,这些函数与具体硬件无关,属顶层调用函数。
  • USB调试接口函数集:包括cdcacm.h和cdcacm.c共2个文件。提供了bootloader程序基于USB接口进行调试时所需的输入输出函数。
  • 串口调试接口集:包括usart.h和usart.c共2个文件。提供了bootloader程序基于串口进行调试时所需的输入输出函数。
  • main函数:包括main_f1.c、main_f3.c、main_f4.c、main_f7.c共4个文件。分别对应STM32F1、STM32F3、STM32F4和STM32F7芯片上运行的Bootloader的主函数。
  • 链接文件:以.ld结尾,包括stm32f1.ld、stm32f3.ld、stm32f4.ld、stm32f7.ld共4个文件。分别对应STM32F1、STM32F3、STM32F4和STM32F7芯片上Bootloader程序的链接方式。
  • PX固件生成脚本:包括px_mkfw.py共1个文件。根据文件说明,PX4固件为基于JSON编码的python对象,此脚本用于生成包含额外编码域的压缩文件固件,类似于u-boot中mkimage程序,生成的固件上传前需要用此脚本进行打包。PX4的固件本身已经具有这项功能,这个脚本应该不再需要了,应该很快会被丢弃。
  • PX固件上传脚本:包括px_uploader.py共1个文件。根据说明,此文件不再使用,已被固件中的上传脚本取代。

综上所述,这里需要重点分析的文件包括Makefile文件,链接文件,main函数文件,硬件配置头文件(hw_config.h),通用bootloader调用函数集文件(bl.h和bl.c),USB虚拟串口函数集文件(cdcacm.h和cdcacm.c),串口函数集文件(包括usart.h和usart.c)。

3 Makefile文件解析

PX4 Bootloader工程包含5个Makefile文件。其中Makefile是make命令的入口文件,将根据不同的编译对象,调用不同的Makefile.*文件。为了更清晰地了解Bootloader的编译过程,本节将对工程的Makefile进行详细注释。

3.1 主Makefile文件注释

Bootloader编译的make命令入口为根目录下的Makefile,这是所有编译命令开始的文件。


Makefile


    
    
  1. #
  2. # Paths to common dependencies
  3. #
  4. export BL_BASE ?= $(wildcard .) #获取Bootloader本地工程所在目录,并赋给变量BL_BASE
  5. export LIBOPENCM3 ?= $(wildcard libopencm3) #获取开源库libopencm3所在目录,并赋给变量LIBOPENCM3
  6. #
  7. # Tools
  8. #
  9. export CC = arm-none-eabi-gcc #定义交叉编译工具链变量CC
  10. export OBJCOPY = arm-none-eabi-objcopy #定义二进制生成工具链变量OBJCOPY
  11. #
  12. # Common configuration
  13. #
  14. export FLAGS = -std=gnu99 \ #使用GNU99的优化C语言标准
  15. -Os \ #专门针对生成目标文件大小进行
  16. -g \ #生成调试信息
  17. -Wundef \ #当没有定义的符号出现在#if中时警告
  18. -Wall \ #打开一些有用的警告选项
  19. -fno-builtin \ #不接受没有__buildin__前缀的函数作为内建函数
  20. -I$(LIBOPENCM3)/include \ #包含开源库libopencm3的头文件
  21. -ffunction-sections \ #要求编译器为每个function分配独立的section
  22. -nostartfiles \ #链接时不使用标准的启动文件
  23. -lnosys \ #链接libnosys.a文件
  24. -Wl,-gc-sections \ #传递-gc-sections给连接器,删除没有使用的section
  25. -Wl,-g \ #传递-g选项给链接器,兼容其他工具
  26. -Werror #把警告当错误,出现警告就放弃编译
  27. export COMMON_SRCS = bl.c cdcacm.c usart.c #定义通用源文件变量COMMON_SRCS
  28. #
  29. # Bootloaders to build
  30. #
  31. TARGETS = \ #定义编译目标
  32. aerofcv1_bl \
  33. auavx2v1_bl \
  34. crazyflie_bl \
  35. mindpxv2_bl \
  36. px4aerocore_bl \
  37. px4discovery_bl \
  38. px4flow_bl \
  39. px4fmu_bl \
  40. px4fmuv2_bl \
  41. px4fmuv4_bl \
  42. px4fmuv4pro_bl \
  43. px4fmuv5_bl \
  44. px4io_bl \
  45. px4iov3_bl \
  46. tapv1_bl
  47. all: $(TARGETS) #编译目标all=$(TARGETS)
  48. clean: #定义清理工程的操作(即make clean)
  49. cd libopencm3 && make --no-print-directory clean && cd .. # 从Bootloader本地工程目录进入libopencm3文件夹,执行make clean清理命令,然后回到上一级目录。
  50. rm -f *.elf *.bin #删除.efl和.bin文件
  51. #
  52. # Specific bootloader targets.
  53. # 各编译目标的具体操作,主要规定了编译需要使用的Makefile文件,硬件目标TARGET_HW,链接脚本LINKER_FILE,编译目标TARGET_FILE_NAME
  54. #
  55. auavx2v1_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  56. make -f Makefile.f4 TARGET_HW=AUAV_X2V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  57. px4fmu_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  58. make -f Makefile.f4 TARGET_HW=PX4_FMU_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  59. px4fmuv2_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  60. make -f Makefile.f4 TARGET_HW=PX4_FMU_V2 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  61. px4fmuv4_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  62. make -f Makefile.f4 TARGET_HW=PX4_FMU_V4 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  63. px4fmuv4pro_bl:$(MAKEFILE_LIST) $(LIBOPENCM3)
  64. make -f Makefile.f4 TARGET_HW=PX4_FMU_V4_PRO LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@ EXTRAFLAGS=-DSTM32F469
  65. px4fmuv5_bl:$(MAKEFILE_LIST) $(LIBOPENCM3)
  66. make -f Makefile.f7 TARGET_HW=PX4_FMU_V5 LINKER_FILE=stm32f7.ld TARGET_FILE_NAME=$@
  67. mindpxv2_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  68. make -f Makefile.f4 TARGET_HW=MINDPX_V2 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  69. px4discovery_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  70. make -f Makefile.f4 TARGET_HW=PX4_DISCOVERY_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  71. px4flow_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  72. make -f Makefile.f4 TARGET_HW=PX4_FLOW_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  73. px4aerocore_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  74. make -f Makefile.f4 TARGET_HW=PX4_AEROCORE_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  75. crazyflie_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  76. make -f Makefile.f4 TARGET_HW=CRAZYFLIE LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  77. # Default bootloader delay is *very* short, just long enough to catch
  78. # the board for recovery but not so long as to make restarting after a
  79. # brownout problematic.
  80. #
  81. px4io_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  82. make -f Makefile.f1 TARGET_HW=PX4_PIO_V1 LINKER_FILE=stm32f1.ld TARGET_FILE_NAME=$@
  83. px4iov3_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  84. make -f Makefile.f3 TARGET_HW=PX4_PIO_V3 LINKER_FILE=stm32f3.ld TARGET_FILE_NAME=$@
  85. tapv1_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  86. make -f Makefile.f4 TARGET_HW=TAP_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  87. aerofcv1_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  88. make -f Makefile.f4 TARGET_HW=AEROFC_V1 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@
  89. #
  90. # Binary management
  91. # 二进制文件压缩操作:deploy命令
  92. #
  93. .PHONY: deploy
  94. deploy:
  95. zip Bootloader.zip *.bin
  96. #
  97. # Submodule management
  98. # 子模块libopencm3操作与管理,进行的操作如下:
  99. # 1. 更新git子工程libopencm3。
  100. # 2. 调用Tools/check_submodules.sh检查当前libopencm3的版本信息是否正确。
  101. # 3. 编译libopencm3工程,生成库文件,供bootloader使用。
  102. #
  103. $(LIBOPENCM3): checksubmodules
  104. make -C $(LIBOPENCM3) lib
  105. .PHONY: checksubmodules
  106. checksubmodules: updatesubmodules
  107. $(Q) ($(BL_BASE)/Tools/check_submodules.sh)
  108. .PHONY: updatesubmodules
  109. updatesubmodules:
  110. $(Q) (git submodule init)
  111. $(Q) (git submodule update)

3.2 主控Makefile.f4文件注释

由主Makefile中目标生成语句可以看出,Pixhawk V2硬件系统的主控stm32f427对应的bootloader调用了如下语句:


Makefile


    
    
  1. px4fmuv2_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  2. make -f Makefile.f4 TARGET_HW=PX4_FMU_V2 LINKER_FILE=stm32f4.ld TARGET_FILE_NAME=$@

因此,目标px4fmuv2_bl生成的Makefile脚本为Makefile.f4,同时,它还定义了如下变量

  • 硬件目标:$(TARGET_HW)=PX4_FMU_V2
  • 链接文件:$(LINKER_FILE)=stm32f4.ld
  • 目标名称:$(TARGET_FILE_NAME)=$@=px4fmuv2_bl

同时,在主Makefile中规定的全局变量BL_BASE、LIBOPENCM3、CC、OBJCOPY、FLAGS、COMMON_SRCS在Makefile.f4中同样有效。下面将对Makefile.f4进行详细注释。


Makefile.f4


    
    
  1. OPENOCD ?= openocd #定义OPENOCD命令
  2. JTAGCONFIG ?= interface/olimex-jtag-tiny.cfg #定义openocd对应的jtag烧写器配置文件
  3. #JTAGCONFIG ?= interface/jtagkey-tiny.cfg
  4. # 5 seconds / 5000 ms default delay
  5. PX4_BOOTLOADER_DELAY ?= 5000 #定义启动变量PX4_BOOTLOADER_DELAY为5000ms
  6. SRCS = $(COMMON_SRCS) main_f4.c #定义编译源文件为bl.c cdcacm.c usart.c和main_f4.c
  7. FLAGS += -mthumb\ #使用thumb指令集
  8. -mcpu=cortex-m4\ #使用cortex m4对应的CPU指令
  9. -mfloat-abi=hard\ #浮点为硬件浮点运算
  10. -mfpu=fpv4-sp-d16 \ #浮点运算协处理器为fpv4-sp-d16
  11. -DTARGET_HW_$(TARGET_HW) \ #定义变量TARGET_HW_PX4_FMU_V2为1
  12. -DSTM32F4 \ #定义变量STM32F4为1
  13. -T$(LINKER_FILE) \ #使用链接文件stm32f4.ld
  14. -L$(LIBOPENCM3)/lib \ #添加库文件搜索目录libopencm3/lib
  15. -lopencm3_stm32f4 \ #链接库文件libopencm3_stm32f4.a
  16. $(EXTRAFLAGS)
  17. ELF = $(TARGET_FILE_NAME).elf #定义目标变量ELF=px4fmuv2_bl.elf
  18. BINARY = $(TARGET_FILE_NAME).bin #定义目标二进制变量BINARY=px4fmuv2_bl.bin
  19. all: $(ELF) $(BINARY) #定义目标all,为px4fmuv2_bl.elf和px4fmuv2_bl.bin
  20. #px4fmuv2_bl.elf的生成规则:
  21. #px4fmuv2_bl.elf: bl.c cdcacm.c usart.c main_f4.c Makefile
  22. # arm-none-eabi-gcc -o px4fmuv2_bl.elf bl.c cdcacm.c usart.c main_f4.c $(FLAGS)
  23. $(ELF): $(SRCS) $(MAKEFILE_LIST)
  24. $(CC) -o $@ $(SRCS) $(FLAGS)
  25. #px4fmuv2_bl.bin的生成规则:
  26. #px4fmuv2_bl.bin: px4fmuv2_bl.elf
  27. # arm-none-eabi-objcopy -O binary px4fmuv2_bl.elf px4fmuv2_bl.bin
  28. $(BINARY): $(ELF)
  29. $(OBJCOPY) -O binary $(ELF) $(BINARY)
  30. #upload: all flash flash-bootloader
  31. #定义upload目标,依赖于all flash-bootloader
  32. upload: all flash-bootloader
  33. #定义flash-bootloader目标的命令规则
  34. #执行openocd命令,查找与本工程目录平行的px4_bootloader目录中interface/olimex-jtag-tiny.cfg文件作为jtag烧写器的配置文件,以stm32f4x.cfg作为主控stm32f427的SOC配置文件。然后在openocd环境下依次执行init, reset halt, flash write_image erase px4fmuv2_bl.bin 0x08000000, reset run, shutdown命令进行烧写。
  35. flash-bootloader:
  36. $(OPENOCD) --search ../px4_bootloader -f $(JTAGCONFIG) -f stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase $(BINARY) 0x08000000" -c "reset run" -c shutdown
  37. # Use to upload to a stm32f4-discovery devboard, requires the latest version of openocd (from git)
  38. # build openocd with "cd openocd; ./bootstrap; ./configure --enable-maintainer-mode --enable-stlink"
  39. #定义upload-discovery目标的命令规则
  40. #执行openocd命令,默认使用stlink烧写器及其配置文件,查找board/stm32f4discovery.cfg配置文件,在openocd环境下依次执行init, reset halt, flash probe 0, stm32f2x mass_erase 0, flash write_image erase px4fmuv2_bl.bin 0x08000000, reset, shutdown命令进行烧写。
  41. upload-discovery:
  42. $(OPENOCD) --search ../px4_bootloader -f board/stm32f4discovery.cfg -c init -c "reset halt" -c "flash probe 0" -c "stm32f2x mass_erase 0" -c "flash write_image erase $(BINARY) 0x08000000" -c "reset" -c shutdown

3.3 IO协处理器Makefile.f1文件注释

由主Makefile中目标生成语句可以看出,Pixhawk V2硬件系统的IO协处理器stm32f100对应的bootloader调用了如下语句:


Makefile


    
    
  1. px4io_bl: $(MAKEFILE_LIST) $(LIBOPENCM3)
  2. make -f Makefile.f1 TARGET_HW=PX4_PIO_V1 LINKER_FILE=stm32f1.ld TARGET_FILE_NAME=$@

因此,目标px4io_bl生成的Makefile脚本为Makefile.f1,同时,它还定义了如下变量

  • 硬件目标:$(TARGET_HW)=PX4_PIO_V1
  • 链接文件:$(LINKER_FILE)=stm32f1.ld
  • 目标名称:$(TARGET_FILE_NAME)=$@=px4io_bl

同理,在主Makefile中规定的全局变量BL_BASE、LIBOPENCM3、CC、OBJCOPY、FLAGS、COMMON_SRCS在Makefile.f1中同样有效。下面将对Makefile.f1进行详细注释。


Makefile.f1


    
    
  1. OPENOCD ?= ../../sat/bin/openocd #定义openocd命令变量
  2. JTAGCONFIG ?= interface/olimex-jtag-tiny.cfg #定义Jtag烧写器配置文件
  3. #JTAGCONFIG ?= interface/jtagkey-tiny.cfg
  4. # 3 seconds / 3000 ms default delay
  5. PX4_BOOTLOADER_DELAY ?= 3000 #定义bootloader延时3000ms
  6. SRCS = $(COMMON_SRCS) main_f1.c #定义源文件为bl.c cdcacm.c usart.c和main_f1.c
  7. FLAGS += -mthumb\ #使用thumb指令集
  8. -mcpu=cortex-m3\ #使用cortex m3指令集
  9. -DTARGET_HW_$(TARGET_HW) \ #定义TARGET_HW_PX4_PIO_V1变量为1
  10. -DSTM32F1 \ #定义STM32F1变量为1
  11. -T$(LINKER_FILE) \ #使用链接脚本stm32f1.ld
  12. -L$(LIBOPENCM3)/lib \ #增加库文件搜索目录libopencm3/lib
  13. -lopencm3_stm32f1 #链接库文件libopencm3_stm32f1.a
  14. ELF = $(TARGET_FILE_NAME).elf #定义ELF目标变量px4io_bl.elf
  15. BINARY = $(TARGET_FILE_NAME).bin #定义BINARY目标变量px4io_bl.bin
  16. all: $(ELF) $(BINARY) #定义目标all为px4io_bl.elf和px4io_bl.bin
  17. #px4io_bl.elf生成规则
  18. #px4io_bl.elf: bl.c cdcacm.c usart.c main_f1.c Makefile
  19. # arm-none-eabi-gcc -o px4io_bl.elf bl.c cdcacm.c usart.c main_f1.c $(FLAGS)
  20. $(ELF): $(SRCS) $(MAKEFILE_LIST)
  21. $(CC) -o $@ $(SRCS) $(FLAGS)
  22. #px4io_bl.bin生成规则
  23. #px4io_bl.bin: px4io_bl.elf
  24. # arm-none-eabi-objcopy -O binary px4io_bl.elf px4io_bl.bin
  25. $(BINARY): $(ELF)
  26. $(OBJCOPY) -O binary $(ELF) $(BINARY)
  27. #upload: all flash flash-bootloader
  28. #定义upload目标,依赖于all flash-bootloader
  29. upload: all flash-bootloader
  30. #flash-bootloader目标生成命令
  31. #执行openocd命令,查找与本工程目录平行的px4_bootloader目录中interface/olimex-jtag-tiny.cfg文件作为jtag烧写器的配置文件,以stm32f1.cfg作为IO协处理器stm32f100的SOC配置文件。然后在openocd环境下依次执行init, reset halt, flash write_image erase px4io_bl.bin, reset run, shutdown命令进行烧写。
  32. flash-bootloader:
  33. $(OPENOCD) --search ../px4_bootloader -f $(JTAGCONFIG) -f stm32f1.cfg -c init -c "reset halt" -c "flash write_image erase $(BINARY)" -c "reset run" -c shutdown

4 链接文件解析

PX4 Bootloader中包含4个链接文件stm32f1.ld、stm32f3.ld、stm32f4.ld和stm32f7.ld。与pixhawk V2硬件相关的为主控stm32f4.ld和IO协处理器stm32f1.ld两个链接脚本,它们将作为本文的分析重点。

PX4 Bootloader的链接文件可分为如下4个部分:

  • 声明可使用存储结构变量
  • 声明向量表名称
  • 定义段区
  • 定义栈顶地址变量值

4.1 主控链接脚本stm32f4.ld解析

链接脚本stm32f4.ld和libopencm3/lib/cm3/vector.c文件之间的关系十分密切。stm32f4.ld中定义了一系列在vector.c文件中使用的变量,包括_data_loadaddr,_data,_edata,_ebss,_stack,__preinit_array_start,__preinit_array_end,__init_array_start,__init_array_end,__fini_array_start,__fini_array_end,这些变量被用于Cortex M4的向量表初始化,主要是第一项的堆栈指针初始化和第二项的reset_handler函数的初始化。链接脚本stm32f4.ld中声明的向量表变量vector_table的定义在vector.c文件中。

与其它常见的链接脚本不同,stm32f4.ld中没有关键字ENTRY来规定程序的入口地址,在这种情况下程序就是从代码段(text)开始的。代码段最开始位置的是向量表,且ST公司的Cortex M4内核默认入口首地址(0x0)为堆栈指针(SP寄存器)的值;第二字为入口地址,因此reset_handler函数被调用为程序入口地址。


stm32f4.ld


    
    
  1. /* Define memory regions. */
  2. /* 声明存储区域,分为rom和ram两部分 */
  3. MEMORY
  4. {
  5. rom (rx) : ORIGIN = 0x08000000, LENGTH = 16K /* 定义rom区,即片内flash的部分区域,权限可读可执行,起始于flash起始地址0x08000000,使用前16K(即Section 0) */
  6. ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K /* 定义ram区,即片内SRAM的部分区域,权限可读可写可执行,起始于SRAM1起始地址0x20000000,包含SRAM1和SRAM2的全部区域(128K) */
  7. }
  8. /* Enforce emmission of the vector table. */
  9. /* 声明外部定义的vector_table变量,此变量被定义在libopencm3/lib/cm3/vector.c中 */
  10. EXTERN (vector_table)
  11. /* Define sections. */
  12. SECTIONS /* 定义段 */
  13. {
  14. . = ORIGIN(rom); /* 链接器指针定位于rom区起始地址 */
  15. .text : { /* 定义代码段text */
  16. *(.vectors) /* Vector table 中断向量表,定义于libopencm3/lib/cm3/vector.c文件中,仅包含vector_table变量 */
  17. *(.text*) /* Program code 程序代码段 */
  18. . = ALIGN( 4); /* 4字节对齐 */
  19. *(.rodata*) /* Read-only data 程序只读数据段 */
  20. . = ALIGN( 4); /* 4字节对齐 */
  21. _etext = .; /* 定义变量_etext为当前地址 */
  22. } >rom /* 代码段text的程序地址位于rom中 */
  23. /* C++ Static constructors/destructors, also used for __attribute__
  24. * ((constructor)) and the likes */
  25. .preinit_array : { /* 定义C++构造函数段preinit_array */
  26. . = ALIGN( 4); /* 4字节对齐 */
  27. __preinit_array_start = .; /* 定义变量__preinit_array_start为当前地址 */
  28. KEEP (*(.preinit_array)) /* 强制链接器保留preinit_array段 */
  29. __preinit_array_end = .; /* 定义变量__preinit_array_end为当前地址 */
  30. } >rom /* C++构造函数段preinit_array的程序地址位于rom中 */
  31. .init_array : { /* 定义C++构造函数段init_array */
  32. . = ALIGN( 4); /* 4字节对齐 */
  33. __init_array_start = .; /* 定义变量__init_array_start为当前地址 */
  34. KEEP (*(SORT(.init_array.*))) /* 强制链接器保留init_array.* 段,并对满足字符串模式的内容进行升序排列 */
  35. KEEP (*(.init_array)) /* 强制链接器保留init_array段 */
  36. __init_array_end = .; /* 定义变量__init_array_end为当前地址 */
  37. } >rom /* C++构造函数段init_array的程序地址位于rom中 */
  38. .fini_array : { /* 定义C++析构函数段fini_array */
  39. . = ALIGN( 4); /* 4字节对齐 */
  40. __fini_array_start = .; /* 定义变量__fini_array_start为当前地址 */
  41. KEEP (*(.fini_array)) /* 强制链接器保留fini_array段 */
  42. KEEP (*(SORT(.fini_array.*))) /* 强制链接器保留fini_array.*段,并对满足字符串模式的内容进行排序 */
  43. __fini_array_end = .; /* 定义变量__fini_array_end为当前地址 */
  44. } >rom /* C++析构函数段fini_array的程序地址位于rom中 */
  45. . = ORIGIN(ram); /* 链接器指针定位于ram区起始地址 */
  46. .data : AT(_etext) { /* 定义数据段data,加载地址位于变量_etext定义的位置 */
  47. _data = .; /* 定义变量_data为当前地址 */
  48. *(.data*) /* Read-write initialized data,程序数据段 */
  49. . = ALIGN( 4); /* 4字节对齐 */
  50. _edata = .; /* 定义变量_edata为当前地址 */
  51. } >ram /* 数据段data的程序地址位于ram中 */
  52. _data_loadaddr = LOADADDR(.data); /* 定义变量_data_loadaddr为数据段加载地址的起始位置 */
  53. .bss : { /* 定义bss段 */
  54. *(.bss*) /* Read-write zero initialized data,程序bss段 */
  55. *(COMMON) /* COMMON段 */
  56. . = ALIGN( 4); /* 4字节对齐 */
  57. _ebss = .; /* 定义变量_ebss为当前地址 */
  58. } >ram AT >rom /* bss段的程序地址位于ram中,加载地址位于rom中 */
  59. /*
  60. * The .eh_frame section appears to be used for C++ exception handling.
  61. * You may need to fix this if you're using C++.
  62. */
  63. /DISCARD/ : { *(.eh_frame) } /* 被丢弃段eh_frame不会出现在输出文件中 */
  64. . = ALIGN( 4); /* 4字节对齐 */
  65. end = .; /* 定义变量end为当前地址 */
  66. }
  67. PROVIDE(_stack = 0x20020000); /* 定义变量_stack,仅在被使用时生效,堆栈指针指向ram区最高地址 */

4.2 IO协处理器链接脚本stm32f1.ld解析

链接脚本stm32f1.ld与stm32f4.ld情况十分相近,不再复述,这里仅对其进行注释。


stm32f1.ld


    
    
  1. /* Define memory regions. */
  2. /* 声明存储区域,分为rom和ram两部分 */
  3. MEMORY
  4. {
  5. rom (rx) : ORIGIN = 0x08000000, LENGTH = 4K /* 定义rom区,即片内flash的部分区域,权限可读可执行,起始于flash起始地址0x08000000,使用前4K(即Page 0~3) */
  6. ram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K /* 定义ram区,即片内SRAM的部分区域,权限可读可写可执行,起始于SRAM1起始地址0x20000000,使用全部8K(注意F100系列片内RAM最高为8K) */
  7. }
  8. /* Enforce emmission of the vector table. */
  9. /* 声明外部定义的vector_table变量,此变量被定义在libopencm3/lib/cm3/vector.c中 */
  10. EXTERN (vector_table)
  11. /* Define sections. */
  12. SECTIONS /* 定义段 */
  13. {
  14. . = ORIGIN(rom); /* 链接器指针定位于rom区起始地址 */
  15. .text : { /* 定义代码段text */
  16. *(.vectors) /* Vector table 中断向量表,定义于libopencm3/lib/cm3/vector.c文件中,仅包含vector_table变量 */
  17. *(.text*) /* Program code 程序代码段 */
  18. . = ALIGN( 4); /* 4字节对齐 */
  19. *(.rodata*) /* Read-only data 程序只读数据段 */
  20. . = ALIGN( 4); /* 4字节对齐 */
  21. _etext = .; /* 定义变量_etext为当前地址 */
  22. } >rom /* 代码段text的程序地址位于rom中 */
  23. /* C++ Static constructors/destructors, also used for __attribute__
  24. * ((constructor)) and the likes */
  25. .preinit_array : { /* 定义C++构造函数段preinit_array */
  26. . = ALIGN( 4); /* 4字节对齐 */
  27. __preinit_array_start = .; /* 定义变量__preinit_array_start为当前地址 */
  28. KEEP (*(.preinit_array)) /* 强制链接器保留preinit_array段 */
  29. __preinit_array_end = .; /* 定义变量__preinit_array_end为当前地址 */
  30. } >rom /* C++构造函数段preinit_array的程序地址位于rom中 */
  31. .init_array : { /* 定义C++构造函数段init_array */
  32. . = ALIGN( 4); /* 4字节对齐 */
  33. __init_array_start = .; /* 定义变量__init_array_start为当前地址 */
  34. KEEP (*(SORT(.init_array.*))) /* 强制链接器保留init_array.* 段,并对满足字符串模式的内容进行升序排列 */
  35. KEEP (*(.init_array)) /* 强制链接器保留init_array段 */
  36. __init_array_end = .; /* 定义变量__init_array_end为当前地址 */
  37. } >rom /* C++构造函数段init_array的程序地址位于rom中 */
  38. .fini_array : { /* 定义C++析构函数段fini_array */
  39. . = ALIGN( 4); /* 4字节对齐 */
  40. __fini_array_start = .; /* 定义变量__fini_array_start为当前地址 */
  41. KEEP (*(.fini_array)) /* 强制链接器保留fini_array段 */
  42. KEEP (*(SORT(.fini_array.*))) /* 强制链接器保留fini_array.* 段,并对满足字符串模式的内容进行升序排列 */
  43. __fini_array_end = .; /* 定义变量__fini_array_end为当前地址 */
  44. } >rom /* C++析构函数段fini_array的程序地址位于rom中 */
  45. . = ORIGIN(ram); /* 链接器指针定位于ram区起始地址 */
  46. .data : AT(_etext) { /* 定义数据段data,加载地址位于变量_etext定义的位置 */
  47. _data = .; /* 定义变量_data为当前地址 */
  48. *(.data*) /* Read-write initialized data 程序数据段 */
  49. . = ALIGN( 4); /* 4字节对齐 */
  50. _edata = .; /* 定义变量_edata为当前地址 */
  51. } >ram /* 数据段data的程序地址位于ram中 */
  52. _data_loadaddr = LOADADDR(.data); /* 定义变量_data_loadaddr为数据段加载地址的起始位置 */
  53. .bss : { /* 定义bss段 */
  54. *(.bss*) /* Read-write zero initialized data 程序bss段 */
  55. *(COMMON) /* COMMON段 */
  56. . = ALIGN( 4); /* 4字节对齐 */
  57. _ebss = .; /* 定义变量_ebss为当前地址 */
  58. } >ram AT >rom /* bss段的程序地址位于ram中,加载地址位于rom中 */
  59. /*
  60. * The .eh_frame section appears to be used for C++ exception handling.
  61. * You may need to fix this if you're using C++.
  62. */
  63. /DISCARD/ : { *(.eh_frame) } /* 被丢弃段eh_frame不会出现在输出文件中 */
  64. . = ALIGN( 4); /* 4字节对齐 */
  65. end = .; /* 定义变量end为当前地址 */
  66. }
  67. PROVIDE(_stack = 0x20002000); /* 定义变量_stack,仅在被使用时生效,堆栈指针指向ram区最高地址 */

5 硬件配置头文件

PX4 Bootloader的硬件配置头文件仅有一个,hw_config.h。这套代码支持的所有硬件板配置均可在这里找到。它的结构很简单,针对不同的硬件配置使用#if-#elif-#end宏条件编译语句进行配置。针对Pixhawk V2这套硬件,仅涉及其中TARGET_HW_PX4_FMU_V2和TARGET_HW_PX4_PIO_V1两个条件编译块中的内容。由于本节将涉及具体的硬件信息,将结合芯片手册和原理图对这两部分内容进行详细解析。

5.1 主控宏TARGET_HW_PX4_FMU_V2对应的配置

在Makefile.f4的FLAGS变量中定义了TARGET_HW_PX4_FMU_V2和STM32F4两个变量,这两个变量主要用于宏条件编译分支的判断上,对硬件板载配置的时候起到了决定性的作用。


Makefile.f4


    
    
  1. FLAGS += -mthumb\ #使用thumb指令集
  2. -mcpu=cortex-m4\ #使用cortex m4对应的CPU指令
  3. -mfloat-abi=hard\ #浮点为硬件浮点运算
  4. -mfpu=fpv4-sp-d16 \ #浮点运算协处理器为fpv4-sp-d16
  5. -DTARGET_HW_$(TARGET_HW) \ #定义变量TARGET_HW_PX4_FMU_V2为1
  6. -DSTM32F4 \ #定义变量STM32F4为1
  7. -T$(LINKER_FILE) \ #使用链接文件stm32f4.ld
  8. -L$(LIBOPENCM3)/lib \ #添加库文件搜索目录libopencm3/lib
  9. -lopencm3_stm32f4 \ #链接库文件libopencm3_stm32f4.a
  10. $(EXTRAFLAGS)

hw_config.h文件中对应主控宏TARGET_HW_PX4_FMU_V2的代码如下:


hw_config.h


    
    
  1. #elif defined(TARGET_HW_PX4_FMU_V2) /* 若TARGET_HW_PX4_FMU_V2为真,下面的宏定义有效 */
  2. # define APP_LOAD_ADDRESS 0x08004000 /* APP_LOAD_ADDRESS,为Bootloader初始化完成后的飞控固件跳转地址(实际入口地址在0x08004004,这一点在bl.c文件的源代码中可以看到,0x08004000被用于标识后面的代码是否有效) */
  3. # define BOOTLOADER_DELAY 5000 /* BOOTLOADER_DELAY,为Bootloader初始化完毕到跳转到飞控固件需等待的时间 */
  4. # define BOARD_FMUV2 /* BOARD_FMUV2,在全部代码中没有用到 */
  5. # define INTERFACE_USB 1 /* INTERFACE_USB,表示Bootloader可采用USB口与上位机通信 */
  6. # define INTERFACE_USART 1 /* INTERFACE_USART,表示Bootloader可采用串口与上位机通信 */
  7. # define USBDEVICESTRING "PX4 BL FMU v2.x" /* USBDEVICESTRING,表示对应USB ID的字符串 */
  8. # define USBPRODUCTID 0x0011 /* USBPRODUCTID,USB接口的PID */
  9. # define BOOT_DELAY_ADDRESS 0x000001a0 /* BOOT_DELAY_ADDRESS,flash上对应存放Bootloader等待时间的内存地址 */
  10. # define BOARD_TYPE 9 /* BOARD_TYPE,对应不同硬件配置板子上处理器的编号,每个不同的处理器对应不同的值 */
  11. # define _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) /* _FLASH_KBYTES,程序运行期间flash以KB计的大小的存储地址(为何位于Reserved地址?) */
  12. # define BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) /* BOARD_FLASH_SECTORS,STM32F4芯片若片内flash为1M,则Sector数量为11;若不为1M(2M),则Sector数量为23,详见芯片手册 */
  13. # define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) /* BOARD_FLASH_SIZE,flash总大小 */
  14. # define OSC_FREQ 24 /* OSC_FREQ,外部晶振频率24kHz */
  15. /* 主控FMU指示灯在板上一共有2个,FMU的PWR和B/E;PWR(LED704)为绿色常亮,上电即亮,B/E(LED701)由FMU的GPIOE12控制,为闪烁红色 */
  16. # define BOARD_PIN_LED_ACTIVITY 0 // no activity LED,无命令处理显示LED灯
  17. # define BOARD_PIN_LED_BOOTLOADER GPIO12 /* BOARD_PIN_LED_BOOTLOADER,主控Bootloader的LED指示灯为GPIOE12 */
  18. # define BOARD_PORT_LEDS GPIOE /* BOARD_PORT_LEDS,主控FMU的LED指示灯控制引脚在GPIO Port E */
  19. # define BOARD_CLOCK_LEDS RCC_AHB1ENR_IOPEEN /* BOARD_CLOCK_LEDS,RCC_AHB1ENR_IOPEEN被libopencm3/include/libopencm3/stm32/f4/rcc.h定义为(1<<4),对应RCC_AHB1ENR寄存器的第4位,GPIOE的时钟使能位 */
  20. # define BOARD_LED_ON gpio_clear /* BOARD_LED_ON,gpio_clear函数定义在libopencm3/lib/stm32/common/gpio_common_all.c中,实际为GPIO pin清零功能 */
  21. # define BOARD_LED_OFF gpio_set /* BOARD_LED_OFF,gpio_set函数定义在libopencm3/lib/stm32/common/gpio_common_all.c中,实际为GPIO pin置1功能 */
  22. # define BOARD_USART USART2 /* BOARD_USART,通信串口,USART2被libopencm3/include/libopencm3/stm32/common/usart_common_all.h定义为USART2_BASE(0x40004400) */
  23. # define BOARD_USART_CLOCK_REGISTER RCC_APB1ENR /* BOARD_USART_CLOCK_REGISTER,被定义为RCC_APB1ENR寄存器,用于使能USART2的时钟 */
  24. # define BOARD_USART_CLOCK_BIT RCC_APB1ENR_USART2EN /* BOARD_USART_CLOCK_BIT,RCC_APB1ENR_USART2EN被libopencm3/include/libopencm3/stm32/f4/rcc.h定义为(1<<17),对应RCC_APB1ENR的17位,用于使能USART2的时钟 */
  25. # define BOARD_PORT_USART GPIOD /* BOARD_PORT_USART,USART2的引脚位于GPIO Port D */
  26. # define BOARD_PORT_USART_AF GPIO_AF7 /* BOARD_PORT_USART_AF,GPIO_AF7被libopencm3/include/libopencm3/stm32/common/gpio_common_f234.h定义为0x7,对应AF7的alternative function功能为USART1~3 */
  27. # define BOARD_PIN_TX GPIO5 /* BOARD_PIN_TX,USART2的发送引脚为GPIOD5 */
  28. # define BOARD_PIN_RX GPIO6 /* BOARD_PIN_RX,USART2的接收引脚为GPIOD6 */
  29. # define BOARD_USART_PIN_CLOCK_REGISTER RCC_AHB1ENR /* BOARD_USART_PIN_CLOCK_REGISTER,被定义为RCC_AHB1ENR寄存器,用于使能USART2引脚的时钟 */
  30. # define BOARD_USART_PIN_CLOCK_BIT RCC_AHB1ENR_IOPDEN /* BOARD_USART_PIN_CLOCK_BIT,RCC_AHB1ENR_IOPDEN被libopencm3/include/libopencm3/stm32/f4/rcc.h定义为(1<<3),对应RCC_AHB1ENR寄存器的第3位,用于使能GPIOD的时钟 */

5.2 IO协处理器宏TARGET_HW_PX4_PIO_V1对应的配置

在Makefile.f1的FLAGS变量中定义了TARGET_HW_PX4_PIO_V1和STM32F1两个变量,这两个变量主要用于宏条件编译分支的判断上,对硬件板载配置的时候起到了决定性的作用。


Makefile.f1


    
    
  1. FLAGS += -mthumb\ #使用thumb指令集
  2. -mcpu=cortex-m3\ #使用cortex m3指令集
  3. -DTARGET_HW_$(TARGET_HW) \ #定义TARGET_HW_PX4_PIO_V1变量为 1
  4. -DSTM32F1 \ #定义STM32F1变量为 1
  5. -T$(LINKER_FILE) \ #使用链接脚本stm32f1.ld
  6. -L$(LIBOPENCM3)/lib \ #增加库文件搜索目录libopencm3/lib
  7. -lopencm3_stm32f1 #链接库文件libopencm3_stm32f1.a

hw_config.h文件中对应主控宏TARGET_HW_PX4_PIO_V1的代码如下:


hw_config.h


    
    
  1. #elif defined(TARGET_HW_PX4_PIO_V1) || defined(TARGET_HW_PX4_PIO_V2) /* 若TARGET_HW_PX4_FMU_V2为真,下面的宏定义有效 */
  2. # define APP_LOAD_ADDRESS 0x08001000 /* APP_LOAD_ADDRESS,为bootloader初始化完成后的飞控固件跳转地址(实际入口地址在0x08001004,这一点在bl.c文件的源代码中可以看到,0x08001000被用于标识后面的代码是否有效) */
  3. # define APP_SIZE_MAX 0xf000 /* APP_SIZE_MAX,板载系统程序(Bootloader)大小上限 */
  4. # define BOOTLOADER_DELAY 200 /* BOOTLOADER_DELAY,为Bootloader初始化完毕到跳转到飞控固件需等待的时间 */
  5. # define BOARD_PIO /* 未见引用 */
  6. # define INTERFACE_USB 0 /* INTERFACE_USB,表示Bootloader不能用USB与上位机通信 */
  7. # define INTERFACE_USART 1 /* INTERFACE_USART,表示Bootloader可以用串口与上位机通信 */
  8. # define USBDEVICESTRING "" /* USBDEVICESTRING,因INTERFACE_USB为0,不使用USB与上位机通信,故USB ID的字符串设置为空 */
  9. # define USBPRODUCTID -1 /* USBPRODUCTID,因INTERFACE_USB为0,不使用USB与上位机通信,故USB设备ID设为非法值 */
  10. # define OSC_FREQ 24 /* OSC_FREQ,外部晶振频率24kHz */
  11. /* IO协处理器指示灯在板上一共有3个,IO的PWR、B/E和ACT;PWR(LED702)为绿色常亮,上电即亮,B/E(LED703)由IO协处理器的GPIOB15控制,为闪烁红色,ACT(LED705)由IO协处理器的GPIOB14控制,为闪烁蓝色 */
  12. # define BOARD_PIN_LED_ACTIVITY GPIO14 /* BOARD_PIN_LED_ACTIVITY,Bootloader有命令要处理时亮起,由IO协处理器的GPIOB14控制 */
  13. # define BOARD_PIN_LED_BOOTLOADER GPIO15 /* BOARD_PIN_LED_BOOTLOADER,IO协处理器Bootloader的LED指示灯为GPIOB15 */
  14. # define BOARD_PORT_LEDS GPIOB /* BOARD_PORT_LEDS,IO协处理器的LED指示灯控制引脚在GPIO Port B */
  15. # define BOARD_CLOCK_LEDS_REGISTER RCC_APB2ENR /* BOARD_CLOCK_LEDS_REGISTER,被定义为RCC_APB2ENR寄存器,用于使能LED时钟 */
  16. # define BOARD_CLOCK_LEDS RCC_APB2ENR_IOPBEN /* BOARD_CLOCK_LEDS,RCC_APB2ENR_IOPBEN被libopencm3/include/libopencm3/stm32/f1/rcc.h定义为(1<<3),对应RCC_APB2ENR寄存器的第3,GPIOB的时钟使能位 */
  17. # define BOARD_LED_ON gpio_clear /* BOARD_LED_ON,gpio_clear函数定义在libopencm3/lib/stm32/common/gpio_common_all.c中,实际为GPIO pin清零功能 */
  18. # define BOARD_LED_OFF gpio_set /* BOARD_LED_OFF,gpio_set函数定义在libopencm3/lib/stm32/common/gpio_common_all.c中,实际为GPIO pin置1功能 */
  19. /* IO协处理器的通信串口为USART2,与主控FMU的USART6通信 */
  20. # define BOARD_USART USART2 /* BOARD_USART,通信串口,USART2被libopencm3/include/libopencm3/stm32/common/usart_common_all.h定义为USART2_BASE(0x40004400) */
  21. # define BOARD_USART_CLOCK_REGISTER RCC_APB1ENR /* BOARD_USART_CLOCK_REGISTER,被定义为RCC_APB1ENR寄存器,用于使能USART2的时钟 */
  22. # define BOARD_USART_CLOCK_BIT RCC_APB1ENR_USART2EN /* BOARD_USART_CLOCK_BIT,RCC_APB1ENR_USART2EN被libopencm3/include/libopencm3/stm32/f1/rcc.h定义为(1<<17),对应RCC_APB1ENR的17位,用于使能USART2的时钟 */
  23. # define BOARD_PORT_USART GPIOA /* BOARD_PORT_USART,USART2的引脚位于GPIO Port A */
  24. # define BOARD_PIN_TX GPIO_USART2_TX /* BOARD_PIN_TX,GPIO_USART2_TX被libopencm3/include/libopencm3/stm32/f1/gpio.h中定义为GPIO2,PA2 */
  25. # define BOARD_PIN_RX GPIO_USART2_RX /* BOARD_PIN_RX,GPIO_USART2_RX被libopencm3/include/libopencm3/stm32/f1/gpio.h中定义为GPIO3,PA3 */
  26. # define BOARD_USART_PIN_CLOCK_REGISTER RCC_APB2ENR /* BOARD_USART_PIN_CLOCK_REGISTER,被定义为寄存器RCC_APB2ENR,用于使能USART2引脚的时钟 */
  27. # define BOARD_USART_PIN_CLOCK_BIT RCC_APB2ENR_IOPAEN /* BOARD_USART_PIN_CLOCK_BIT,RCC_APB2ENR_IOPAEN被libopencm3/include/libopencm3/stm32/f1/rcc.h定义为(1<<2),对应RCC_APB2ENR的第2位,用于使能GPIOA的时钟 */
  28. /* IO协处理器的GPIOB5接J702安全开关 */
  29. # define BOARD_FORCE_BL_PIN GPIO5 /* BOARD_FORCE_BL_PIN,板载强制Bootloader引脚为GPIOB5 */
  30. # define BOARD_FORCE_BL_PORT GPIOB /* BOARD_FORCE_BL_PIN,板载强制Bootloader引脚在GPIOB中 */
  31. # define BOARD_FORCE_BL_CLOCK_REGISTER RCC_APB2ENR /* BOARD_FORCE_BL_CLOCK_REGISTER,被定义为RCC_APB2ENR,用于使能GPIOB5的时钟 */
  32. # define BOARD_FORCE_BL_CLOCK_BIT RCC_APB2ENR_IOPBEN /* BOARD_FORCE_BL_CLOCK_BIT,RCC_APB2ENR_IOPBEN被libopencm3/include/libopencm3/stm32/f1/rcc.h定义为(1<<3),对应RCC_APB2ENR寄存器的第3,GPIOB的时钟使能位 */
  33. # define BOARD_FORCE_BL_PULL GPIO_CNF_INPUT_FLOAT // depend on external pull BOARD_FORCE_BL_PULL,GPIO_CNF_INPUT_FLOAT被libopencm3/include/libopencm3/stm32/f1/gpio.h定义为0x01,根据芯片手册表示GPIOB5为浮动输入状态,其值取决于外部硬件输入,即安全开关的状态
  34. # define BOARD_FORCE_BL_VALUE BOARD_FORCE_BL_PIN /* BOARD_FORCE_BL_VALUE,BOARD_FORCE_BL_PIN为GPIO5的值,用于判定IO协处理器是否永远进入Bootloader的循环状态 */
  35. # define BOARD_FLASH_SECTORS 60 /* BOARD_FLASH_SECTORS,STM32F1片内flash页数,Bootloader认为片内flash的前60页有效 */
  36. # define BOARD_TYPE 10 /* BOARD_TYPE,对应不同硬件配置板子上处理器的编号,每个不同的处理器对应不同的值 */
  37. # define FLASH_SECTOR_SIZE 0x400 /* FLASH_SECTOR_SIZE,STM32F1片内flash每页大小为1KB */

6 核心数据结构

与u-boot类似,PX4 Bootloader运行时需要一些核心数据结构的支持。这些数据结构一般是全局变量,它们的值能够充分反映整个程序运行的状态。Pixhawk V2板上有两个处理器分别跑着各自的Bootloader,因此各自维持这自己的核心数据结构。无论主控FMU还是IO协处理器的Bootloader程序都是从向量表开始的,而且PX4 Bootloader对所有处理器的向量表处理方式完全相同,因此本节将向量表数据结构单独拿出来分析。

6.1 向量表vector_table_t

STM32的Cortex M核心启动时均需要中断向量表支持,因此这个数据结构对程序的启动至关重要。这个数据结构在库文件libopencm3/include/libopencm3/cm3/vector.h中定义,宏NVIC_IRQ_COUNT在libopencm3/include/libopencm3/dispatch/nvic.h中被定义为0,故一般中断向量irq数量为0,可理解为不启用一般中断。


vector.h


    
    
  1. typedef void (*vector_table_entry_t)(void); /* 定义向量表函数指针类型 */
  2. typedef struct {
  3. unsigned int *initial_sp_value; /* 初始堆栈指针地址 */
  4. vector_table_entry_t reset; /* 重启reset向量函数指针 */
  5. vector_table_entry_t nmi; /* 不可屏蔽中断NMI向量函数指针 */
  6. vector_table_entry_t hard_fault; /* 硬件错误hard_fault向量函数指针 */
  7. vector_table_entry_t memory_manage_fault; /* 内存管理错误memory_manage_fault向量函数指针,Cortex M0核心不含此项 */
  8. vector_table_entry_t bus_fault; /* 总线错误bus_fault向量函数指针,Cortex M0核心不含此项 */
  9. vector_table_entry_t usage_fault; /* 指令使用错误usage_fault向量函数指针,Cortex M0核心不含此项 */
  10. vector_table_entry_t reserved_x001c[ 4]; /* 预留 */
  11. vector_table_entry_t sv_call; /* 管理模式sv_call向量函数指针 */
  12. vector_table_entry_t debug_monitor; /* 调试监控debug_monitor向量函数指针,Cortex M0核心不含此项(手册显示M4和M3也没有) */
  13. vector_table_entry_t reserved_x0034; /* 预留 */
  14. vector_table_entry_t pend_sv; /* pend_sv向量函数指针 */
  15. vector_table_entry_t systick; /* 系统时钟systick向量函数指针 */
  16. vector_table_entry_t irq[NVIC_IRQ_COUNT]; /* 一般中断irq向量函数指针数组,宏NVIC_IRQ_COUNT被定义为0 */
  17. } vector_table_t;

vector_table_t数据结构在文件libopencm3/lib/cm3/vector.c中实体化为变量vector_table并被初始化,单独构成vectors段,被链接脚本规定放在代码段最开始位置。初始化的重点时栈指针initial_sp_value和重启reset向量函数reset_handler,其余或者被初始化为什么都不做的null_handler,或者被初始化为死循环函数blocking_handler。null_handler和blocking_handler的定义均在vector.c文件中。宏IRQ_HANDLERS在文件libopencm3/lib/dispatch/vector_nvic.c中被定义为空,说明Bootloader不含中断处理函数。

vector.c中,向量函数的初始化函数均被定义为弱属性(weak),表示若外部没有同名函数定义,则用此弱属性函数;若有其他定义,则采用其他定义的函数。从这一点也可看到,对Bootloader的向量函数进行重写时十分方便,不需要修改软件框架。由于reset_handeler作为入口函数,因此还增加了裸属性(naked)。


vector.c


    
    
  1. __attribute__ ((section( ".vectors"))) /* 变量vector_table属于vectors段 *
  2. vector_table_t vector_table = {
  3. .initial_sp_value = &_stack, /* 栈指针指向的地址,_stack在链接脚本中被定义为ram区最高地址 */
  4. .reset = reset_handler, /* 重启reset向量函数,reset_handler被定义在vector.c中 */
  5. .nmi = nmi_handler, /* 不可屏蔽中断nmi向量函数,nmi_handeler在vector.c中被定义为什么都不做的函数null_handler */
  6. .hard_fault = hard_fault_handler, /* 硬件错误hard_fault向量函数,hard_fault_handler在vector.c中被定义为死循环函数blocking_handler */
  7. /* Those are defined only on CM3 or CM4 */
  8. # if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__) /* 若为CM3和CM4 */
  9. .memory_manage_fault = mem_manage_handler, /* 内存管理错误memory_manage_fault向量函数,mem_manage_handler在vector.c中被定义为死循环函数blocking_handler */
  10. .bus_fault = bus_fault_handler, /* 总线错误bus_fault向量函数,bus_fault_handler在vector.c中被定义为死循环函数blocking_handler */
  11. .usage_fault = usage_fault_handler, /* 指令使用错误usage_fault向量函数,usage_fault_handler在vector.c中被定义为死循环函数blocking_handler */
  12. .debug_monitor = debug_monitor_handler, /* 调试监控debug_monitor向量函数,debug_monitor_handler在vector.c中被定义为什么都不做的函数null_handler */
  13. #endif
  14. .sv_call = sv_call_handler, /* 调试监控debug_monitor向量函数,sv_call_handler在vector.c中被定义为什么都不做的函数null_handler */
  15. .pend_sv = pend_sv_handler, /* pend_sv向量函数,pend_sv_handler在vector.c中被定义为什么都不做的函数null_handler */
  16. .systick = sys_tick_handler, /* 系统时钟systick向量函数,sys_tick_handler在vector.c中被定义为什么都不做的函数null_handler */
  17. /* 由于sys_tick_handler被vector.c定义为弱属性(weak),该函数在bl.c中被重新定义,实际定义在bl.c中。 */
  18. .irq = {
  19. IRQ_HANDLERS /* IRQ_HANDLERS,在vector_nvic.c中被定义为空 */
  20. }
  21. };

6.2 主控FMU核心数据结构

除向量表vector外,主控FMU的核心数据结构包括boardinfo,flash_sector,mcu_des_t和mcu_rev_t,它们均为全局变量,表示板载FMU的基本配置情况。

6.2.1 结构体boardinfo

结构体boardinfo被定义在bl.h文件中,在main_f4.c中被初始化,用于描述运行此Bootloader的板载系统的最基本配置。


bl.h


    
    
  1. struct boardinfo {
  2. uint32_t board_type; /* 板载处理器编号 */
  3. uint32_t board_rev; /* 修订版本,程序中几乎没有用到 */
  4. uint32_t fw_size; /* 飞控固件大小的最大值 */
  5. uint32_t systick_mhz; /* 系统时钟的输入值,即CPU主频 */
  6. } __attribute__((packed)); /* 编译时使用紧凑数据模式,不进行优化对齐,即使用1字节对齐模式 */


main_f4.c


    
    
  1. struct boardinfo board_info = {
  2. .board_type = BOARD_TYPE, /* board_type,BOARD_TYPE板载处理器编号在hw_config.h中定义,主控为9。 */
  3. .board_rev = 0, /* board_rev,修订版本为0 */
  4. .fw_size = 0, /* fw_size,飞控固件大小的最大值,这里的0没有意义,在board_init函数中被重新初始化 */
  5. .systick_mhz = 168, /* systick_mhz,系统时钟输入,即CPU主频168MHz */
  6. };

6.2.2 结构体flash_sector

结构体flash_sector用于描述STM32F4芯片内部flash的结构,它被定义并初始化在main_f4.c中。根据芯片手册,STM32F4片内flash大小为1M或2M两种,具体参见芯片手册。


main_f4.c


    
    
  1. static struct {
  2. uint32_t sector_number;
  3. uint32_t size;
  4. } flash_sectors[] = {
  5. { 0x01, 16 * 1024},
  6. { 0x02, 16 * 1024},
  7. { 0x03, 16 * 1024},
  8. { 0x04, 64 * 1024},
  9. { 0x05, 128 * 1024},
  10. { 0x06, 128 * 1024},
  11. { 0x07, 128 * 1024},
  12. { 0x08, 128 * 1024},
  13. { 0x09, 128 * 1024},
  14. { 0x0a, 128 * 1024},
  15. { 0x0b, 128 * 1024},
  16. /* flash sectors only in 2MiB devices */
  17. { 0x10, 16 * 1024},
  18. { 0x11, 16 * 1024},
  19. { 0x12, 16 * 1024},
  20. { 0x13, 16 * 1024},
  21. { 0x14, 64 * 1024},
  22. { 0x15, 128 * 1024},
  23. { 0x16, 128 * 1024},
  24. { 0x17, 128 * 1024},
  25. { 0x18, 128 * 1024},
  26. { 0x19, 128 * 1024},
  27. { 0x1a, 128 * 1024},
  28. { 0x1b, 128 * 1024},
  29. };

6.2.3 结构体mcu_des_t

结构体mcu_des_t存储了STM32F4类MCU的所有型号信息,它被定义在main_f4.c文件中,并被实体化为变量数组mcu_descriptions,此结构用于程序中自动识别当前MCU类型。MCU类型信息被存储在DBGMCU_IDCODE寄存器中,地址为0xE0042000。此结构体在check_silicon函数中使用。


main_f4.c


    
    
  1. // address of MCU IDCODE
  2. #define DBGMCU_IDCODE 0xE0042000
  3. #define STM32_UNKNOWN 0
  4. #define STM32F40x_41x 0x413
  5. #define STM32F42x_43x 0x419
  6. #define STM32F42x_446xx 0x421
  7. typedef struct mcu_des_t {
  8. uint16_t mcuid;
  9. const char *desc;
  10. char rev;
  11. } mcu_des_t;
  12. mcu_des_t mcu_descriptions[] = {
  13. { STM32_UNKNOWN, "STM32F???", '?'},
  14. { STM32F40x_41x, "STM32F40x", '?'},
  15. { STM32F42x_43x, "STM32F42x", '?'},
  16. { STM32F42x_446xx, "STM32F446XX", '?'},
  17. };

6.2.4 结构体mcu_rev_t

结构体mcu_rev_t存储了MCU的版本信息,它被定义在main_f4.c文件中,并被实体化为变量数组silicon_revs,此结构除了可以识别处理器版本外,还可以据此判断MCU的内部flash信息。


main_f4.c


    
    
  1. typedef enum mcu_rev_e { /* 版本号枚举定义,感觉不全啊, */
  2. MCU_REV_STM32F4_REV_A = 0x1000, /* REV A for STM32F405/407/415/417/42X/43X */
  3. MCU_REV_STM32F4_REV_Z = 0x1001, /* REV Z for STM32F405/407/415/417 */
  4. MCU_REV_STM32F4_REV_Y = 0x1003, /* REV Y for STM32F42X/43X */
  5. MCU_REV_STM32F4_REV_1 = 0x1007, /* REV 1 for STM32F42X/43X */
  6. MCU_REV_STM32F4_REV_3 = 0x2001 /* REV 3 for STM32F42X/43X */
  7. } mcu_rev_e;
  8. typedef struct mcu_rev_t {
  9. mcu_rev_e revid;
  10. char rev;
  11. } mcu_rev_t;
  12. const mcu_rev_t silicon_revs[] = {
  13. {MCU_REV_STM32F4_REV_3, '3'}, /* Revision 3 */
  14. {MCU_REV_STM32F4_REV_A, 'A'}, /* Revision A */ // FIRST_BAD_SILICON_OFFSET (place good ones above this line and update the FIRST_BAD_SILICON_OFFSET accordingly)
  15. {MCU_REV_STM32F4_REV_Z, 'Z'}, /* Revision Z */
  16. {MCU_REV_STM32F4_REV_Y, 'Y'}, /* Revision Y */
  17. {MCU_REV_STM32F4_REV_1, '1'}, /* Revision 1 */
  18. };

6.3 IO协处理器核心数据结构

除向量表vector外,IO协处理器的核心数据结构仅有结构体boardinfo,它表征了IO协处理器的运行状态。它同样定义在bl.h中,在main_f1.c中被初始化。


main_f1.c


    
    
  1. struct boardinfo board_info = {
  2. .board_type = BOARD_TYPE, /* board_type,BOARD_TYPE板载处理器编号在hw_config.h中定义,IO协处理器为10。 */
  3. .board_rev = 0, /* board_rev,修订版本为0 */
  4. .fw_size = APP_SIZE_MAX, /* fw_size,APP_SIZE_MAX飞控固件大小的最大值,在hw_config.h中被定义为0xf000,60KB */
  5. .systick_mhz = OSC_FREQ, /* systick_mhz,系统时钟输入等于OSC_FREQ,在hw_config.h中被定义为24MHz */
  6. };

7 PX4 Bootloader主线程序

铺垫了这么多,重点终于来了。与u-boot类似,无论是在主控FMU还是IO协处理器上,PX4 Bootloader本身只是一个规模较大的单片机程序,它的主要功能有:

  • 初始化板载芯片并跳转到飞控固件入口
  • 提供板载调试功能,便于系统调试

由于Pixhawk V2硬件的主控FMU和IO协处理器分别运行着自己的那套Bootloader代码,因此它将有2个Bootloader主线。这两个主线程序的引导(main函数之前)由libopencm3库提供支持,premain阶段的代码是相同的。

7.1 初始化阶段(reset_handler函数)程序

Pixhawk V2的主控FMU芯片型号为STM32F427,内核为Cortex M4;IO协处理器芯片的型号为STM32F100,内核为Cortex M3,因此它们启动后MCU的入口地址为向量表的第二项。向量表的内容参见本文“核心数据结构”一节中关于向量表的内容(文件libopencm3/lib/cm3/vector.c),本节的重点是reset_handler函数的详细解析。

reset_handler函数的主要功能如下:

  • 用数据段存储的参数初始化内存空间
  • 清零BSS段
  • 设置异常处理堆栈指针对齐模式
  • 调用与架构相关的pre_main函数
  • 运行所有构造函数
  • 调用main函数
  • 调用所有析构函数

libopencm3/lib/cm3/vector.c


    
    
  1. /* 变量声明 */
  2. extern unsigned _data_loadaddr, _data, _edata, _ebss, _stack; /* 段初始化变量,定义在链接脚本中 */
  3. typedef void (*funcp_t) (void); /* 定义函数指针类型funcp_t,其原型为void (*functionname) (void) */
  4. extern funcp_t __preinit_array_start, __preinit_array_end; /* C++构造函数地址变量 */
  5. extern funcp_t __init_array_start, __init_array_end; /* C++构造函数地址变量 */
  6. extern funcp_t __fini_array_start, __fini_array_end; /* C++析构函数地址变量 */
  7. /* 定义为弱属性,可直接在其他地方重写此函数;定义为裸属性,确保编译器对reset_handler函数不添加任何加载和卸载代码 */
  8. void __attribute__ ((weak, naked)) reset_handler( void)
  9. {
  10. volatile unsigned *src, *dest;
  11. funcp_t *fp;
  12. /* 用数据段存储的参数初始化内存空间,变量定义见链接脚本。 */
  13. /* _data_loadaddr:数据段的起始加载地址,即在flash上的首地址 */
  14. /* _data:数据段的起始运行地址,即在RAM中的首地址 */
  15. /* _edata:数据段的截止运行地址,即在RAM中的末地址 */
  16. for (src = &_data_loadaddr, dest = &_data; dest < &_edata; src++, dest++) {
  17. *dest = *src;
  18. }
  19. /* BSS段清零,变量定义见各自的链接脚本 */
  20. /* _ebss:bss段的截止运行地址,其起始运行地址紧接数据段的末地址 */
  21. while (dest < &_ebss) {
  22. *dest++ = 0;
  23. }
  24. /* 确保进入异常模式时堆栈指针8字节对齐。此设置对M3和R1架构不起作用 */
  25. SCB_CCR |= SCB_CCR_STKALIGN;
  26. /* 调用与架构相关的pre_main函数(主控FMU在文件libopencm3/lib/stm32/f4/vector_chipset.c中;IO协处理器在文件libopencm3/lib/cm3/vector.c中,且为空函数)。 */
  27. pre_main();
  28. /* 依次调用所有C++定义的构造函数,变量定义见链接脚本。 */
  29. /* __preinit_array_start:preinit_array段首地址 */
  30. /* __preinit_array_end:preinit_array段末地址 */
  31. /* __init_array_start:init_array段首地址 */
  32. /* __init_array_end:init_array段末地址 */
  33. for (fp = &__preinit_array_start; fp < &__preinit_array_end; fp++) {
  34. (*fp)();
  35. }
  36. for (fp = &__init_array_start; fp < &__init_array_end; fp++) {
  37. (*fp)();
  38. }
  39. /* 调用main函数(主控FMU在文件main_f4.c中,IO协处理器在文件main_f1.c中)。 */
  40. main();
  41. /* 依次调用所有C++定义的析构函数,变量定义见链接脚本 */
  42. /* __fini_array_start:fini_array段首地址 */
  43. /* __fini_array_end:fini_array段末地址 */
  44. for (fp = &__fini_array_start; fp < &__fini_array_end; fp++) {
  45. (*fp)();
  46. }
  47. }

本小节结束前,再简单提一下pre_main函数。该函数只对主控FMU有意义,被定义在文件libopencm3/lib/stm32/f4/vector_chipset.c中,IO协处理器中的pre_main是个空函数。

主控FMU的pre_main函数只做了一件事:使能硬件浮点运算


libopencm3/lib/stm32/f4/vector_chipset.c


    
    
  1. static void pre_main(void)
  2. {
  3. /* 使能硬件浮点运算 */
  4. SCB_CPACR |= SCB_CPACR_FULL * (SCB_CPACR_CP10 | SCB_CPACR_CP11);
  5. }

7.2 主控FMU的main函数

主控FMU的主线程序的核心是main函数,它被入口函数reset_handler调用。main函数的主要功能是初始化主控FMU芯片并启动飞控固件;若成功代码将永远运行飞控程序,若失败则可与上位机通信方便调试飞控板。主控FMU的main函数流程如下:

  1. 开启浮点运算功能(premain函数中已开启)。
  2. 调用board_init函数来初始化board_info结构体、GPIOA9(VBUS)端口、USART2端口、LED灯等。
  3. 调用clock_init函数来设置时钟、flash访问。
  4. 检查寄存器BOOT_RTC_REG中的值是否与宏BOOT_RTC_SIGNATURE相同。若相同则停留在bootloader。中,若不相同则可以跳转至飞控固件
  5. 通过检查飞控固件在flash地址0x080041a0和0x080041a4的值,来判定是否停留在bootloader中。
  6. 检查USB是否连接,若连接则必须等待timeout再跳转到飞控固件。
  7. 检测串口是否收到break信号(字节0),若收到需等待timeout时间再跳转到飞控固件。
  8. 若运行到此处try_boot依然为真,则调用函数jump_to_app启动飞控固件---------------------------------------------跳转,若函数返回(跳转失败)则继续9步。
  9. 若函数jump_to_app返回,跳转失败;设置寄存器BOOT_RTC_REG值确保下次启动能够检测到,设置timeout使程序永远停留在bootloader中。
  10. 现在bootloader首次启动飞控固件失败,初始化上位机通信接口(USB和USART2)。
  11. 调用bootloader函数与上位机通信,可运行各种调试命令、烧写新的固件等。
  12. 若串口USART2收到break信号(0字节),跳转到11步。
  13. 调用函数jump_to_app启动飞控固件---------------------------------------------跳转,若函数返回(跳转失败)则timeout赋值为0且跳到11步。

值得注意的是,main函数调用的jump_to_app函数依然有很多操作。这部分代码在bl.c文件中定义,其流程具有通用性,但jump_to_app调用的函数又与架构相关,因此这部分将放在后续中。


main_f4.c


    
    
  1. #if INTERFACE_USART /* 宏INTERFACE_USART定义为1,代码有效,hw_config.h */
  2. # define BOARD_INTERFACE_CONFIG_USART (void *)BOARD_USART /* 宏BOARD_USART定义为USART2,hw_config.h */
  3. #endif
  4. #if INTERFACE_USB /* 宏INTERFACE_USB定义为1,代码有效,hw_config.h */
  5. # define BOARD_INTERFACE_CONFIG_USB NULL
  6. #endif
  7. int main(void)
  8. {
  9. bool try_boot = true; /* bootloader是否立即跳转到飞控固件入口的标志(不等待timeout) */
  10. unsigned timeout = BOOTLOADER_DELAY; /* bootloader初始化完毕后跳转到飞控固件入口地址时,所需等待的时间,以ms计算。宏BOOTLOADER_DELAY在hw_config.h中被定义为5000 */
  11. SCB_CPACR |= (( 3UL << 10 * 2) | ( 3UL << 11 * 2)); /* 使能浮点运算,其功能与pre_main函数相同,实际上可以去掉。 */
  12. #if defined(BOARD_POWER_PIN_OUT) /* 宏BOARD_POWER_PIN_OUT未定义,下面函数不编译 */
  13. if (board_get_rtc_signature() == POWER_DOWN_RTC_SIGNATURE) {
  14. board_set_rtc_signature( 0);
  15. while ( 1);
  16. }
  17. #endif
  18. /* 调用board_init函数,此函数在main_f4.c中被定义,功能如下: */
  19. /* 1. 赋值board_info.fw_size,确定飞控固件size允许的最大值 */
  20. /* 2. 初始化USB端口GPIOA9(VBUS) */
  21. /* 3. 初始化串口USART2 */
  22. /* 4. 初始化LED并点亮(B/E,LED701) */
  23. /* 5. 使能能耗控制器时钟 */
  24. board_init();
  25. /* 调用clock_init函数,此函数在main_f4.c中被定义,功能如下: */
  26. /* 1. PLL时钟设置,并选择main PLL作为系统时钟sysclk,fsysclk=fPLL=168MHz,fUSBSDRNG=48MHz */
  27. /* 2. AHB、APB1、APB2时钟设置,fAHB=fsysclk=168MHz,fAPB1=42MHz,fAPB2=84MHz */
  28. /* 3. 选择高能耗模式(Scale 2 mode) */
  29. /* 4. flash访问控制设置,启动Icache和Dcache,并设置等待时间5周期 */
  30. /* 5. 更新库libopencm3中AHB,APB1,APB2对应的全局变量 */
  31. clock_init();
  32. /* 检查寄存器BOOT_RTC_REG中的值是否与宏BOOT_RTC_SIGNATURE相同,若相同则停留在bootloader中,若不相同则可以跳转至飞控固件 */
  33. /* BOOT_RTC_REG:RTC_BKPxR的第一个32位寄存器,重启不改变存储值,定义在main_f4.c */
  34. /* BOOT_RTC_SIGNATURE:值0xb007b007,被定义在main_f4.c中 */
  35. /* board_get_rtc_signature:被定义在main_f4.c中,用于获取寄存器BOOT_RTC_REG的存储值 */
  36. /* board_set_rtc_signature:被定义在main_f4.c中,设置寄存器BOOT_RTC_REG的值 */
  37. if (board_get_rtc_signature() == BOOT_RTC_SIGNATURE) {
  38. /* bootloader跳转标志为假,不再立即跳转到飞控固件 */
  39. try_boot = false;
  40. /* timeout清零,若无新的飞控固件上传,不再跳转至飞控固件 */
  41. timeout = 0;
  42. /* 寄存器BOOT_RTC_REG清零,确保下次重启可以跳转至飞控固件 */
  43. board_set_rtc_signature( 0);
  44. }
  45. #ifdef BOOT_DELAY_ADDRESS /* 宏BOOT_DELAY_ADDRESS在hw_config.h中有定义,下列代码有效 */
  46. {
  47. /* 这里给定一个机会,通过飞控固件自身的设置可以影响bootloader的行为。 */
  48. /* 设置地址在0x080041a0和0x080041a4的flash值满足一系列逻辑要求,可以防止bootloader自动跳转到飞控固件,给调试工作带来方便。 */
  49. /* BOOT_DELAY_ADDRESS:值0x000001a0,定义在hw_config.h */
  50. /* flash_func_read_word:定义在main_f4.c,用于读取飞控固件内某地址 */
  51. /* 这里,sig1为flash中地址在0x080041a0的32位值,sig2为flash中地址在0x080041a4的32位地址值,在飞控固件地址范围内 */
  52. uint32_t sig1 = flash_func_read_word(BOOT_DELAY_ADDRESS);
  53. uint32_t sig2 = flash_func_read_word(BOOT_DELAY_ADDRESS + 4);
  54. /* BOOT_DELAY_SIGNATURE1:值0x92c2ecff,定义在bl.h */
  55. /* BOOT_DELAY_SIGNATURE2:值0xc5057d5d,定义在bl.h */
  56. /* BOOT_DELAY_MAX:值30,定义在bl.h */
  57. /* 在满足一系列逻辑下,设定try_boot为假,且重新设置timeout,保证跳转到飞控固件前等待timeout时间 */
  58. if (sig2 == BOOT_DELAY_SIGNATURE2 && (sig1 & 0xFFFFFF00) == (BOOT_DELAY_SIGNATURE1 & 0xFFFFFF00)) {
  59. unsigned boot_delay = sig1 & 0xFF;
  60. if (boot_delay <= BOOT_DELAY_MAX) {
  61. try_boot = false;
  62. if (timeout < boot_delay * 1000) {
  63. timeout = boot_delay * 1000;
  64. }
  65. }
  66. }
  67. }
  68. #endif
  69. /* board_test_force_pin:定义在main_f4.c,若bootloader引脚为真,则设置try_boot为假,程序不立即跳转至飞控固件,需等待timeout时间 */
  70. /* 由于函数中条件编译的3个宏(BOARD_FORCE_BL_PIN_OUT、BOARD_FORCE_BL_PIN_IN和BOARD_FORCE_BL_PIN)均未被定义,此函数返回恒为假 */
  71. if (board_test_force_pin()) {
  72. try_boot = false;
  73. }
  74. /* 检查USB是否连接,若连接则必须等待timeout再跳转到飞控固件,否则立即跳转。这样的设置为飞控固件烧写以及调试带来极大方便。 */
  75. #if INTERFACE_USB /* 宏INTERFACE_USB定义在hw_config.h,以下代码有效 */
  76. #if defined(BOARD_USB_VBUS_SENSE_DISABLED) /* 宏BOARD_USB_VBUS_SENSE_DISABLED未定义 */
  77. try_boot = false;
  78. #else
  79. /* GPIOA:地址为0x40020000的寄存器(libopencm3/include/libopencm3/stm32/common/gpio_common_f234.h) */
  80. /* GPIO9:值1<<9(libopencm3/include/libopencm3/stm32/common/gpio_common_all.h) */
  81. /* gpio_get:获取某GPIO组的值,定义在libopencm3/lib/stm32/common/gpio_common_all.c */
  82. /* 根据原理图,GPIOA9对应VBUS/3.1A,高电平表示USB已连接,低电平表示USB未接 */
  83. if (gpio_get(GPIOA, GPIO9) != 0) {
  84. /* 设置try_boot为假,bootloader不会立即跳转至飞控固件,需等待timeout时间 */
  85. try_boot = false;
  86. }
  87. #endif
  88. #endif
  89. /* 检测串口是否收到break信号(字节0),若收到置try_boot为假,需等待timeout时间再跳转到飞控固件 */
  90. #if INTERFACE_USART /* 宏INTERFACE_USB定义在hw_config.h,以下代码有效 */
  91. /* board_test_usart_receiving_break:定义在main_f4.c,连续接收3个字符,若串口上受到字节0则返回真,否则返回假 */
  92. if (board_test_usart_receiving_break()) {
  93. try_boot = false;
  94. }
  95. #endif
  96. /* 若运行到此处try_boot依然为真,则立即跳转到飞控固件(实际上jump_to_app函数代码依然不短,这里可以近似这样认为); */
  97. /* 若跳转未成功,则设置RTC备份寄存器BOOT_RTC_REG为预定值,确保下次重启若不更新飞控固件依然无法跳转。 */
  98. if (try_boot) {
  99. #ifdef BOARD_BOOT_FAIL_DETECT /* 宏BOARD_BOOT_FAIL_DETECT未定义,以下代码无效 */
  100. board_set_rtc_signature(BOOT_RTC_SIGNATURE);
  101. #endif
  102. /* jump_to_app:定义在bl.c,跳转到飞控固件 */
  103. jump_to_app();
  104. /* BOOT_RTC_SIGNATURE: */
  105. /* board_set_rtc_signature: */
  106. /* 设置RTC备份寄存器BOOT_RTC_REG值为BOOT_RTC_SIGNATURE,下次重启检测到后若不更新飞控固件则不跳转,程序一直运行在bootloader中。 */
  107. board_set_rtc_signature(BOOT_RTC_SIGNATURE);
  108. /* 置timeout为0,程序一直在bootloader中 */
  109. timeout = 0;
  110. }
  111. /* 现在bootloader首次启动飞控固件失败,初始化上位机通信接口(USB和USART2) */
  112. #if INTERFACE_USART /* 宏INTERFACE_USART定义在hw_config.h,下列代码有效 */
  113. /* BOARD_INTERFACE_CONFIG_USART:值USART2(值0x40004400,指向USART2寄存器首地址)。 */
  114. /* BOARD_INTERFACE_CONFIG_USART在main_f4.c中被定义为BOARD_USART,BOARD_USART在hw_config.h中被定义为USART2。*/
  115. /* USART2被定义为USART2_BASE,值0x40004400,指向USART2寄存器首地址(libopencm3/include/libopencm3/stm32/common/usart_common_all.h)。 */
  116. /* USART:值1,枚举型,定义在bl.h */
  117. /* cinit:定义在bl.c,用于初始化通信端口 */
  118. /* 初始化串口USART2为与上位机的通信接口 */
  119. cinit(BOARD_INTERFACE_CONFIG_USART, USART);
  120. #endif
  121. #if INTERFACE_USB /* 宏INTERFACE_USB定义在hw_config.h,下列代码有效 */
  122. /* BOARD_INTERFACE_CONFIG_USB:值NULL,定义在main_f4.c */
  123. /* USB:值2,枚举型,定义在bl.h */
  124. /* cinit:定义在bl.c,用于初始化通信端口 */
  125. /* 初始化USB为虚拟串口作为与上位机的通信接口 */
  126. cinit(BOARD_INTERFACE_CONFIG_USB, USB);
  127. #endif
  128. #if 0 /* 下列代码无效 */
  129. // MCO1/02
  130. gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8);
  131. gpio_set_output_options(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO8);
  132. gpio_set_af(GPIOA, GPIO_AF0, GPIO8);
  133. gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9);
  134. gpio_set_af(GPIOC, GPIO_AF0, GPIO9);
  135. #endif
  136. while ( 1) {
  137. /* bootloader:bootloader与上位机的命令处理函数,烧写新的固件或者timeout(不为0)时间到返回,定义在bl.c中 */
  138. bootloader(timeout);
  139. /* board_test_force_pin:定义在main_f4.c,若bootloader引脚为真,则继续循环 */
  140. /* 由于函数中条件编译的3个宏(BOARD_FORCE_BL_PIN_OUT、BOARD_FORCE_BL_PIN_IN和BOARD_FORCE_BL_PIN)均未被定义,此函数返回恒为假 */
  141. if (board_test_force_pin()) {
  142. continue;
  143. }
  144. #if INTERFACE_USART /* 宏INTERFACE_USART定义在hw_config.h,下列代码有效 */
  145. /* board_test_usart_receiving_break:连续接收3个字节的内容,如果收到一个0字节则返回真,否则返回假。定义在main_f4.c */
  146. /* 若串口USART2收到break信号(0字节),继续循环 */
  147. if (board_test_usart_receiving_break()) {
  148. continue;
  149. }
  150. #endif
  151. #ifdef BOARD_BOOT_FAIL_DETECT /* 宏BOARD_BOOT_FAIL_DETECT未定义,下列代码无效 */
  152. board_set_rtc_signature(BOOT_RTC_SIGNATURE);
  153. #endif
  154. /* jump_to_app:跳转至飞控固件 */
  155. jump_to_app();
  156. /* 跳转失败,timeout设置为0,永久停留在bootloader中 */
  157. timeout = 0;
  158. }
  159. }

7.2 IO协处理器的main函数

IO协处理器的主线程序的核心是main函数,它被入口函数reset_handler调用。main函数的主要功能是初始化主控FMU芯片并启动飞控固件;若成功代码将永远运行飞控程序,若失败则可与上位机通信方便调试飞控板。IO协处理器的main函数流程如下:

  1. 调用board_init函数初始化,包括LED灯、强制Bootloader引脚GPIOB5、准备RTC备份寄存器、设置引脚GPIOA2为USART2-TX
  2. 判定,若RTC备份寄存器BKP_DR1中存储了预定值,则timeout赋值为200ms
  3. 若GPIO5为高电平,则timeout赋值为0xffffffff,永久停留在bootloader中
  4. 若timeout为0,则调用jump_to_app立即启动飞控固件---------------------------------------------启动;若jump_to_app函数返回,则timeout赋值为0
  5. 现在bootloader首次启动飞控固件失败,初始化系统时钟为PLL(使用HSI)
  6. 函数初始化串口USART2
  7. 调用bootloader函数与上位机通信,可运行各种调试命令、烧写新的固件等
  8. 调用函数jump_to_app启动飞控固件---------------------------------------------跳转,若函数返回(跳转失败)则timeout赋值为0且跳到7步。

值得注意的是,main函数调用的jump_to_app函数依然有很多操作。这部分代码在bl.c文件中定义,其流程具有通用性,但jump_to_app调用的函数又与架构相关,因此这部分将放在后续中。



    
    
  1. #ifdef INTERFACE_USART /* 宏INTERFACE_USART定义为1,代码有效,hw_config.h */
  2. # define BOARD_INTERFACE_CONFIG (void *)BOARD_USART /* 宏BOARD_USART定义为USART2,hw_config.h */
  3. #else
  4. # define BOARD_INTERFACE_CONFIG NULL
  5. #endif
  6. int main(void)
  7. {
  8. unsigned timeout = 0; /* bootloader初始化完毕后跳转到飞控固件入口地址时,所需等待的时间,以ms计算。 */
  9. /* 调用board_init函数,此函数在main_f1.c中被定义,功能如下 */
  10. /* 1. 初始化LED控制并点亮LED灯 */
  11. /* 2. 设置强制Bootloader引脚GPIOB5为浮动输入模式,方便采集安全开关的状态 */
  12. /* 3. 使能能源接口时钟和备份接口时钟,准备RTC备份寄存器 */
  13. /* 4. 设置USART2-TX引脚GPIOA2,对应原理图SERIAL_IO_TO_FMU,用于IO协处理器发送信息到主控FMU */
  14. board_init();
  15. #if defined(INTERFACE_USART) | defined (INTERFACE_USB) /* 宏INTERFACE_USART和INTERFACE_USB分别被定义为1和0,代码有效,hw_config.h */
  16. timeout = BOOTLOADER_DELAY; /* BOOTLOADER_DELAY:值200,表示200ms,hw_config.h */
  17. #endif
  18. #ifdef INTERFACE_I2C /* 宏INTERFACE_I2C未定义,代码无效 */
  19. # error I2C bootloader detection logic not implemented
  20. #endif
  21. /* 若RTC备份寄存器BKP_DR1中存储了预定值,则timeout赋值为200ms */
  22. /* should_wait:判定RTC备份寄存器BKP_DR1中是否存储了预定值。若存储了返回真,未存储则返回假 */
  23. /* BOOTLOADER_DELAY:值200,表示200ms,hw_config.h */
  24. if (should_wait()) {
  25. timeout = BOOTLOADER_DELAY;
  26. }
  27. #ifdef BOARD_FORCE_BL_PIN /* 宏BOARD_FORCE_BL_PIN定义为GPIO5,代码有效,hw_config.h */
  28. /* 若GPIO5为高电平,则timeout赋值为0xffffffff,永久停留在bootloader中 */
  29. /* BOARD_FORCE_BL_VALUE:被定义为GPIO5(1<<5),hw_config.h */
  30. /* BOARD_FORCE_BL_PORT:被定义为GPIOB(GPIOB的首寄存器),hw_config.h */
  31. /* BOARD_FORCE_BL_PIN:被定义为GPIO5(1<<5),hw_config.h */
  32. /* gpio_get:获取某GPIO组的值,定义在libopencm3/lib/stm32/common/gpio_common_all.c */
  33. if (BOARD_FORCE_BL_VALUE == gpio_get(BOARD_FORCE_BL_PORT, BOARD_FORCE_BL_PIN)) {
  34. timeout = 0xffffffff;
  35. }
  36. #endif
  37. /* 若timeout为0,则调用jump_to_app立即启动飞控固件。 */
  38. /* 若jump_to_app函数返回,则timeout赋值为0 */
  39. if (timeout == 0) {
  40. jump_to_app();
  41. timeout = 0;
  42. }
  43. /* 初始化系统时钟为PLL(使用HSI) */
  44. /* clock_init:使用HSI将MCU的系统时钟为PLL,频率48MHz,定义在main_f1.c */
  45. clock_init();
  46. /* 函数初始化串口USART2 */
  47. /* BOARD_INTERFACE_CONFIG:定义为BOARD_USART(USART2),main_f1.c */
  48. /* USART:值1,枚举型,定义在bl.h中 */
  49. cinit(BOARD_INTERFACE_CONFIG, USART);
  50. while ( 1) {
  51. /* bootloader:bootloader与上位机的命令处理函数,烧写新的固件或者timeout(不为0)时间到返回,定义在bl.c中 */
  52. bootloader(timeout);
  53. /* jump_to_app:跳转至飞控固件 */
  54. jump_to_app();
  55. /* 跳转失败,timeout设置为0,永久停留在bootloader中 */
  56. timeout = 0;
  57. }
  58. }

7.3 飞控固件启动函数jump_to_app

主控FMU或IO协处理器的main函数的一个目标就是调用jump_to_app函数来启动对应的飞控固件。jump_to_app函数定义在文件bl.c中,它的主要功能如下:

  • 通过飞控固件的烧写约定,检测固件是否有效。若无效函数返回,跳转失败
  • 若飞控固件有效,反向初始化MCU外设,准备将外设的控制权交给飞控固件
  • 更改向量表地址至飞控固件的向量表(通过向量表偏移寄存器SCB_VTOR)
  • 重置堆栈并跳转至飞控固件

bl.c


    
    
  1. /* do_jump函数,重置堆栈指针并跳转至飞控固件入口的函数,jump_to_app函数的终极目标 */
  2. static void do_jump(uint32_t stacktop, uint32_t entrypoint)
  3. {
  4. /* 汇编嵌入C语言代码 */
  5. asm volatile(

    1 引言

    半年前入手了Pixhawk V2全套硬件,编译好的开源固件也下了,四轴也飞了,一直想对这套开源飞控进行一个系统地解析,由于工作原因一直没时间。最近翻开了PX4飞控源代码,它基于NUTTX操作系统,在github上更新十分迅速。为了能够全面地掌握这套软硬件设计思想,同时对硬件系统有全面的认识,我决定对PX4 Bootloader进行详细解析。凡涉及到硬件相关的部分,本文以Pixhawk V2的主控STM32F427和IO协处理器STM32F100为基础进行解析,其他硬件可参照此方法进行类比,基本结构都是相似的。

  • 2
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值