pixhawk PX4FMU和PX4IO最底層啟動過程分析
首先，大體瞭解PX4IO 與PX4FMU各自的任務
PX4IO(STM32F100)為PIXHAWK 中專用於處理輸入輸出的部分,輸入為支援的各類遙控器(PPM,SPKT/DSM,SBUS), 輸出為電調的PWM 驅動訊號, 它與PX4FMU(STM32F427)通過串列埠進行通訊
PX4FMU ：各種感測器資料讀取、姿態解算、PWM控制量的計算、與PX4IO通訊
進入主題
PX4IO 與PX4FMU 一樣,在大的軟體架構上分為Bootloader,OS 和APP 三個部分,OS 為Nuttx,如下圖
根據自己理解畫的流程圖：（2016.05.19加）
1.Bootloader
在嵌入式作業系統中，BootLoader是在
因此進入px4/Bootloader可以看到main_f1、main_f4，分別對應著PX4IO、PX4FMU
主要是負責是否升級晶片,不升級則進入OS（具體的內容還沒仔細看，讀者結合註釋應該能夠看懂）
2.Nuttx
程式碼位置：Firmware/build_px4fmu-v2_default/px4fmu-v2/Nuttx/nuttx/arch/arm/src/stm32/stm32_start.c
__start-- #處理器執行的第一條指令（px4使用的是stm32，入口在stm32_start.c中）
|
v
stm32_clockconfig()------ #初始化時鐘
|
v
rcc_reset() #復位rcc
stm32_stdclockconfig() #初始化標準時鍾
rcc_enableperipherals() #使能外設時鐘
------------------------------------------------------------------
stm32_fpuconfig() #配置fpu
|
v
stm32_lowsetup() #基本初始化串列埠，之後可以使用up_lowputc()
stm32_gpioinit() #初始化gpio，只是呼叫stm32_gpioremap()設定重對映
up_earlyserialinit() #初始化串列埠，之後可以使用up_putc()
stm32_boardinitialize()-- #板級初始化
|
v
stm32_spiinitialize() #初始化spi，只是呼叫stm32_configgpio()設定gpio
stm32_usbinitialize() #初始化usb，只是呼叫stm32_configgpio()設定gpio
up_ledinit(); #初始化led，只是呼叫stm32_configgpio()設定gpio
|
----------------------------------------------------------------------------------------------
在stm32_start.c檔案中我們會看到這麼一句話：
os_start()--------------- #初始化作業系統
|
v
dq_init() #初始化各種狀態的任務列表（置為null）
g_pidhash[i]= #初始化唯一可以確定的元素--程序ID
g_pidhash[PIDHASH(0)]= #分配空閒任務的程序ID為0
g_idletcb= #初始化空閒任務的任務控制塊
sem_initialize()-- #初始化訊號量
|
v
dq_init() #將訊號量佇列置為null
sem_initholders() #初始化持有者結構以支援優先順序繼承
|
--------
|
v
up_allocate_heap() #分配使用者模式的堆（設定堆的起點和大小）
kumm_initialize() #初始化使用者模式的堆
up_allocate_kheap() #分配核心模式的堆
kmm_initialize() #初始化核心模式的堆
task_initialize() #初始化任務資料結構
irq_initialize() #將所有中斷向量都指向同一個異常中斷處理程式
wd_initialize() #初始化看門狗資料結構
clock_initialize() #初始化rtc
timer_initialize() #配置POSIX定時器
sig_initialize() #初始化訊號
mq_initialize() #初始化命名訊息佇列
pthread_initialize() #初始化執行緒特定的資料，空函式
fs_initialize()--- #初始化檔案系統
|
v
sem_init() #初始化節點訊號量為1
files_initialize() #初始化檔案陣列，空函式
|
--------
|
v
net_initialize()-- #初始化網路
|
v
uip_initialize() #初始化uIP層
net_initroute() #初始化路由表
netdev_seminit() #初始化網路裝置訊號量
arptimer_init() #初始化ARP定時器
|
--------
|
v
up_initialize()--- #處理器特定的初始化
|
v
up_calibratedelay() #校準定時器
up_addregion() #增加額外的記憶體段
up_irqinitialize() #設定中斷優先順序，關聯硬體異常處理函式
up_pminitialize() #初始化電源管理
up_dmainitialize() #初始化DMA
up_timerinit() #初始化定時器中斷
devnull_register() #註冊/dev/null
devzero_register() #註冊/dev/zero
up_serialinit() #註冊串列埠控制檯/dev/console和串列埠/dev/ttyS0
up_rnginitialize() #初始化並註冊隨機數生成器
up_netinitialize() #初始化網路，是arch/arm/src/chip/stm32_eth.c中的
up_usbinitialize() #初始化usb驅動
board_led_on() #開啟中斷使能led，但很快會被其它地方的led操作改變狀態
|
--------
|
v
lib_initialize() #初始化c庫，空函式
group_allocate() #分配空閒組
group_setupidlefiles() #在空閒任務上建立stdout、stderr、stdin
group_initialize() #完全初始化空閒組
os_bringup()------ #建立初始任務
|
v
KEKERNEL_THREAD() #啟動核心工作者執行緒
board_initialize() #最後一刻的板級初始化
TASK_CREATE() #啟動預設應用程式
|
--------
|
v
forup_idle() #空閒任務迴圈
|
--------------------
|
v
for(;;) #不應該到達這裡
__start 負責STM32 晶片的底層初始化,包括時鐘,FPU,GPIO 等單元的初始化
os_start 負責OS 的底層初始化,包括各種佇列和程序結構的初始化
os_bringup 負責OS 基本程序的啟動以及使用者程序的啟動,使用者啟動入口由(CONFIG_USER_ENTRYPOINT)巨集進行指定
3.APP
PX4FMU中 #define CONFIG_USER_ENTRYPOINT nsh_main
進入nsh_main
int nsh_main(int argc, char *argv[])
{
int exitval = 0;
int ret;
/* Call all C++ static constructors */
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
up_cxxinitialize();
#endif
/* Make sure that we are using our symbol take */
#if defined(CONFIG_LIBC_EXECFUNCS) && defined(CONFIG_EXECFUNCS_SYMTAB)
exec_setsymtab(CONFIG_EXECFUNCS_SYMTAB, 0);
#endif
/* Register the BINFS file system */
#if defined(CONFIG_FS_BINFS) && (CONFIG_BUILTIN)
ret = builtin_initialize();
if (ret < 0)
{
fprintf(stderr, "ERROR: builtin_initialize failed: %d\n", ret);
exitval = 1;
}
#endif
/* Initialize the NSH library */
nsh_initialize();
/* If the Telnet console is selected as a front-end, then start the
* Telnet daemon.
*/
#ifdef CONFIG_NSH_TELNET
ret = nsh_telnetstart();
if (ret < 0)
{
/* The daemon is NOT running. Report the the error then fail...
* either with the serial console up or just exiting.
*/
fprintf(stderr, "ERROR: Failed to start TELNET daemon: %d\n", ret);
exitval = 1;
}
#endif
/* If the serial console front end is selected, then run it on this thread */
#ifdef CONFIG_NSH_CONSOLE
ret = nsh_consolemain(0, NULL);
/* nsh_consolemain() should not return. So if we get here, something
* is wrong.
*/
fprintf(stderr, "ERROR: nsh_consolemain() returned: %d\n", ret);
exitval = 1;
#endif
return exitval;
}#ifdef CONFIG_NSH_CONSOLE
ret = nsh_consolemain(0, NULL);int nsh_consolemain(int argc, char *argv[])
{
FAR struct console_stdio_s *pstate = nsh_newconsole();
int ret;
DEBUGASSERT(pstate);
/* Execute the start-up script */
#ifdef CONFIG_NSH_ROMFSETC
(void)nsh_initscript(&pstate->cn_vtbl);
#endif
/* Initialize any USB tracing options that were requested */
#ifdef CONFIG_NSH_USBDEV_TRACE
usbtrace_enable(TRACE_BITSET);
#endif
/* Execute the session */
ret = nsh_session(pstate);
/* Exit upon return */
nsh_exit(&pstate->cn_vtbl, ret);
return ret;
} /* Execute the start-up script */
#ifdef CONFIG_NSH_ROMFSETC
(void)nsh_initscript(&pstate->cn_vtbl);
#endif /* Execute the session */
ret = nsh_session(pstate);跟蹤pstate
FAR struct console_stdio_s *pstate = nsh_newconsole();FAR struct console_stdio_s *nsh_newconsole(void)
{
struct console_stdio_s *pstate = (struct console_stdio_s *)zalloc(sizeof(struct console_stdio_s));
if (pstate)
{
/* Initialize the call table */
#ifndef CONFIG_NSH_DISABLEBG
pstate->cn_vtbl.clone = nsh_consoleclone;
pstate->cn_vtbl.release = nsh_consolerelease;
#endif
pstate->cn_vtbl.write = nsh_consolewrite;
pstate->cn_vtbl.output = nsh_consoleoutput;
pstate->cn_vtbl.linebuffer = nsh_consolelinebuffer;
pstate->cn_vtbl.redirect = nsh_consoleredirect;
pstate->cn_vtbl.undirect = nsh_consoleundirect;
pstate->cn_vtbl.exit = nsh_consoleexit;
/* (Re-) open the console input device */
#ifdef CONFIG_NSH_CONDEV
pstate->cn_confd = open(CONFIG_NSH_CONDEV, O_RDWR);
if (pstate->cn_confd < 0)
{
free(pstate);
return NULL;
}
/* Create a standard C stream on the console device */
pstate->cn_constream = fdopen(pstate->cn_confd, "r+");
if (!pstate->cn_constream)
{
close(pstate->cn_confd);
free(pstate);
return NULL;
}
#endif
/* Initialize the output stream */
pstate->cn_outfd = OUTFD(pstate);
pstate->cn_outstream = OUTSTREAM(pstate);
}
return pstate;
}PX4IO中 #define CONFIG_USER_ENTRYPOINT user_start
進入user_start
int
user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
ring_blink();
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}controls_tick 負責處理遙控器的輸入內容,包括SBUS 的處理sbus_input、 SPKT/DSM 的處理dsm_port_input、 PPM 的處理ppm_input
PX4IO 底層中斷處理的內容如下圖
（1）紫色為PX4IO 的底層串列埠IO 操作,流程為當PX4IO 收到PX4FMU 的串列埠資料後會執行serial_interrupt, serial_interrupt 負責收發DMA 的操作,如果收到一個完整的包,則呼叫rx_dma_callback 進行處理, rx_dma_callback 首先呼叫rx_handle_packet 解析包中的內容,判斷為寫暫存器還是讀暫存器,處理完成後由rx_dma_callback 傳送回包給PX4FMU
static int
serial_interrupt(int irq, void *context)
{
static bool abort_on_idle = false;
uint32_t sr = rSR; /* get UART status register */
(void)rDR; /* required to clear any of the interrupt status that brought us here */
if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
perf_count(pc_errors);
if (sr & USART_SR_ORE) {
perf_count(pc_ore);
}
if (sr & USART_SR_NE) {
perf_count(pc_ne);
}
if (sr & USART_SR_FE) {
perf_count(pc_fe);
}
/* send a line break - this will abort transmission/reception on the other end */
rCR1 |= USART_CR1_SBK;
/* when the line goes idle, abort rather than look at the packet */
abort_on_idle = true;
}
if (sr & USART_SR_IDLE) {
/*
* If we saw an error, don't bother looking at the packet - it should have
* been aborted by the sender and will definitely be bad. Get the DMA reconfigured
* ready for their retry.
*/
if (abort_on_idle) {
abort_on_idle = false;
dma_reset();
return 0;
}
/*
* The sender has stopped sending - this is probably the end of a packet.
* Check the received length against the length in the header to see if
* we have something that looks like a packet.
*/
unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
/* it was too short - possibly truncated */
perf_count(pc_badidle);
dma_reset();
return 0;
}
/*
* Looks like we received a packet. Stop the DMA and go process the
* packet.
*/
perf_count(pc_idle);
stm32_dmastop(rx_dma);
rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
}
return 0;
}static void
rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
{
/*
* We are here because DMA completed, or UART reception stopped and
* we think we have a packet in the buffer.
*/
perf_begin(pc_txns);
/* disable UART DMA */
rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
/* handle the received packet */
rx_handle_packet();
/* re-set DMA for reception first, so we are ready to receive before we start sending */
dma_reset();
/* send the reply to the just-processed request */
dma_packet.crc = 0;
dma_packet.crc = crc_packet(&dma_packet);
stm32_dmasetup(
tx_dma,
(uint32_t)&rDR,
(uint32_t)&dma_packet,
PKT_SIZE(dma_packet),
DMA_CCR_DIR |
DMA_CCR_MINC |
DMA_CCR_PSIZE_8BITS |
DMA_CCR_MSIZE_8BITS);
stm32_dmastart(tx_dma, NULL, NULL, false);
rCR3 |= USART_CR3_DMAT;
perf_end(pc_txns);
}static void
rx_handle_packet(void)
{
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
/* send a CRC error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xff;
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
/* it's a blind write - pass it on */
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
dma_packet.count_code = PKT_CODE_SUCCESS;
}
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
/* it's a read - get register pointer for reply */
unsigned count;
uint16_t *registers;
if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
/* constrain reply to requested size */
if (count > PKT_MAX_REGS) {
count = PKT_MAX_REGS;
}
if (count > PKT_COUNT(dma_packet)) {
count = PKT_COUNT(dma_packet);
}
/* copy reply registers into DMA buffer */
memcpy((void *)&dma_packet.regs[0], registers, count * 2);
dma_packet.count_code = count | PKT_CODE_SUCCESS;
}
return;
}
/* send a bad-packet error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xfe;
}（2） 藍色為包操作,只提供registers_set 寫操作和registers_get 讀操作
（3）IOPacket 為協議包,包括以下幾部分
count_code 標記包的讀寫,錯誤,長度等資訊
crc 為包的效驗碼
page 為資料頁
offset 為資料偏移量
regs 為資料內容
如果您覺得此文對您的發展有用，請隨意打賞。
您的鼓勵將是筆者書寫高質量文章的最大動力^_^!!