为了做有用的事情,应用需要订阅subscribe输入并发布publish输出 (e.g. 电机 或伺服指令). PX4平台真正的硬件抽象(true hardware abstraction )开始有用——不需要与传感器驱动以任何方式相互作用,并且电路板或者传感器升级后不需要升级。
PX4中应用间独立的消息通道被称作topics. 在这里,感兴趣的是 sensor_combined topic, 其调用整个系统的同步传感器数据。
订阅到topic非常快和干净:
- #include <uORB/topics/sensor_combined.h>
- ..
- int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
sensor_sub_fd
是一个文件描述符,非常高效的执行一块等待新的数据的数据块。当前的进程进入休眠,当有新的数据可用时,由调度程序自动地唤醒,在等待的时候,不占用任何CPU周期。为实现这点,使用poll(),调用 POSIX system 。
将poll()
添加到subscription看上去像:
- #include <poll.h>
- #include <uORB/topics/sensor_combined.h>
- ..
- int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
- /* one could wait for multiple topics with this technique, just using one here */
- struct pollfd fds[] = {
- { .fd = sensor_sub_fd, .events = POLLIN },
- };
- while (true) {
- /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
- int poll_ret = poll(fds, 1, 1000);
- ..
- if (fds[0].revents & POLLIN) {
- /* obtained data for the first file descriptor */
- struct sensor_combined_s raw;
- /* copy sensors raw data into local buffer */
- orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
- printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
- (double)raw.accelerometer_m_s2[0],
- (double)raw.accelerometer_m_s2[1],
- (double)raw.accelerometer_m_s2[2]);
- }
- }
px4_simple_app
的完整代码如下:
- /**
- * @file px4_simple_app.c
- * Minimal application example for PX4 autopilot
- *
- * @author Example User <[email protected]>
- */
- #include <nuttx/config.h>
- #include <unistd.h>
- #include <stdio.h>
- #include <poll.h>
- #include <uORB/uORB.h>
- #include <uORB/topics/sensor_combined.h>
- __EXPORT int px4_simple_app_main(int argc, char *argv[]);
- int px4_simple_app_main(int argc, char *argv[])
- {
- printf("Hello Sky!\n");
- /* subscribe to sensor_combined topic */
- int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
- /* one could wait for multiple topics with this technique, just using one here */
- struct pollfd fds[] = {
- { .fd = sensor_sub_fd, .events = POLLIN },
- /* there could be more file descriptors here, in the form like:
- * { .fd = other_sub_fd, .events = POLLIN },
- */
- };
- int error_counter = 0;
- while (true) {
- /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
- int poll_ret = poll(fds, 1, 1000);
- /* handle the poll result */
- if (poll_ret == 0) {
- /* this means none of our providers is giving us data */
- printf("[px4_simple_app] Got no data within a second\n");
- } else if (poll_ret < 0) {
- /* this is seriously bad - should be an emergency */
- if (error_counter < 10 || error_counter % 50 == 0) {
- /* use a counter to prevent flooding (and slowing us down) */
- printf("[px4_simple_app] ERROR return value from poll(): %d\n"
- , poll_ret);
- }
- error_counter++;
- } else {
- if (fds[0].revents & POLLIN) {
- /* obtained data for the first file descriptor */
- struct sensor_combined_s raw;
- /* copy sensors raw data into local buffer */
- orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
- printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
- (double)raw.accelerometer_m_s2[0],
- (double)raw.accelerometer_m_s2[1],
- (double)raw.accelerometer_m_s2[2]);
- }
- /* there could be more file descriptors here, in the form like:
- * if (fds[1..n].revents & POLLIN) {}
- */
- }
- }
- return 0;
- }
编译app,通过下面的指令:
- make
运行更新后的程序。UORB app 可能已经在运行,但是重新运行一次也没关系,所以执行:
- uorb start
目标请求中间人已经被激活,可以启动传感器:
- sh /etc/init.d/rc.sensors
最后一步是启动应用,但是为什么语法不同?
- px4_simple_app &
不同是,uorb和传感器应用作为后台程序运行Daemon(computing)。 这就允许在不失去对NuttShell控制的情况下,启动/停止其。假如忘了在行末输入& ,应用将会将现在传感器的值铺满显示屏:
- [px4_simple_app] Accelerometer: 0.0483 0.0821 0.0332
- [px4_simple_app] Accelerometer: 0.0486 0.0820 0.0336
- [px4_simple_app] Accelerometer: 0.0487 0.0819 0.0327
- [px4_simple_app] Accelerometer: 0.0482 0.0818 0.0323
- [px4_simple_app] Accelerometer: 0.0482 0.0827 0.0331
- [px4_simple_app] Accelerometer: 0.0489 0.0804 0.0328
由于不是后台程序,没有办法将其停止,只能让其运行,或者通过发起下面的命令重启自驾仪:
- reboot
或者按压PX4FMU上的reset button.如何将应用变为后台程序可以参考this tutorial 。
上一步最后一步是屏幕上铺满传感器数据。通常并不需要数据以最高速率刷新,试图保持最高速率,通常会使整个系统变慢。
API接口非常简单:
- orb_set_interval(int handle, unsigned interval);
限制topic到1 Hz, 只需添加:
- orb_set_interval(sensor_sub_fd, 1000);
到示例代码这行的下面(在line 55附近):
- /* subscribe to sensor_combined topic */
- int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
屏幕将会以1Hz的速率方便的打印传感器数据。
为了使用计算得到的输出,下一步是发布结果。假如我们使用了一个topic,mavlink
app通过该topic转发到地面站,我们甚至可以查看结果。出于这个目的,来拦截姿态 topic。
接口非常简单,初始化topic结构,并通告(advertise)topic。
- #include <uORB/topics/vehicle_attitude.h>
- ..
- /* advertise attitude topic */
- struct vehicle_attitude_s att;
- memset(&att, 0, sizeof(att));
- orb_advert_t att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
在主循环中,当其准备好后,发布信息:
- orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
修改后的完整示例代码为:
运行最终的程序
需要一些基础应用:
- uorb start
- sh /etc/init.d/rc.sensors
- mavlink start -d /dev/ttyS1 -b 115200
最终运行应用:
- px4_simple_app &
假如启动了QGroundControl或者Mission Planner, 可以检查在实时绘图中显示的传感器数据(Main Menu: Main Widget →Realtime Plot),这些数据反映了进程正在发送的数据。
这个教程涉及到了所有需要在PX4自驾仪应用上进行增量开发所需的内容。需要知道的是,在availablehere 这里有完整的topics列表,而且头文件都被写好,并作为参考。
该示例应用在 src/examples/px4_simple_app可用. 在app配置中,注释掉Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk
即可。
/****************************************************************************
* Name: 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;
}
up_cxxinitialize函数我们昨天在 IO找那个已经遇到过了,是用来调用 C++中的静态构造函数的,我们就不去管它了。 “exec_setsymtab”呢缺少一个宏,不会被编译。 “builtin_initialize”函数让我想到了 “g_builtins”数组。
static struct binfmt_s g_builtin_binfmt =
{
NULL, /* next */
builtin_loadbinary, /* load */
};
int builtin_initialize(void)
{
int ret;
/* Register ourselves as a binfmt loader */
bvdbg("Registering Builtin Loader\n");
ret = register_binfmt(&g_builtin_binfmt);
if (ret != 0)
{
bdbg("Failed to register binfmt: %d\n", ret);
}
return ret;
}
int register_binfmt(FAR struct binfmt_s *binfmt)
{
if (binfmt)
{
/* Add the new binary format handler to the head of the list of
* handlers
*/
sched_lock();
binfmt->next = g_binfmts;
g_binfmts = binfmt;
sched_unlock();
return OK;
}
return -EINVAL;
}
这里的初始化可以认为是链表的初始化。只是目前我们没有看到这跟 “g_builtins”数组有什么关系。但如果真的没有猫腻又为什么要这么命名呢,是吧。而答案可能就在 “builtin_loadbinary”函数。
/****************************************************************************
* Name: builtin_loadbinary
*
* Description:
* Verify that the file is an builtin binary.
*
****************************************************************************/
static int builtin_loadbinary(struct binary_s *binp)
{
FAR const char *filename;
FAR const struct builtin_s *b;
int fd;
int index;
int ret;
bvdbg("Loading file: %s\n", binp->filename);
/* Open the binary file for reading (only) */
fd = open(binp->filename, O_RDONLY);
if (fd < 0)
{
int errval = errno;
bdbg("ERROR: Failed to open binary %s: %d\n", binp->filename, errval);
return -errval;
}
/* If this file is a BINFS file system, then we can recover the name of
* the file using the FIOC_FILENAME ioctl() call.
*/
ret = ioctl(fd, FIOC_FILENAME, (unsigned long)((uintptr_t)&filename));
if (ret < 0)
{
int errval = errno;
bdbg("ERROR: FIOC_FILENAME ioctl failed: %d\n", errval);
return -errval;
}
/* Other file systems may also support FIOC_FILENAME, so the real proof
* is that we can look up the index to this name in g_builtins[].
*/
index = builtin_isavail(filename);
if (index < 0)
{
int errval = errno;
bdbg("ERROR: %s is not a builtin application\n", filename);
return -errval;
}
/* Return the load information. NOTE: that there is no way to configure
* the priority. That is a bug and needs to be fixed.
*/
b = builtin_for_index(index);
binp->entrypt = b->main;
binp->stacksize = b->stacksize;
binp->priority = b->priority;
return OK;
}
int builtin_isavail(FAR const char *appname)
{
FAR const char *name;
int i;
for (i = 0; (name = builtin_getname(i)); i++)
{
if (!strncmp(name, appname, NAME_MAX))
{
return i;
}
}
set_errno(ENOENT);
return ERROR;
}
FAR const char *builtin_getname(int index)
{
FAR const struct builtin_s *builtin;
builtin = builtin_for_index(index);
if (builtin != NULL)
{
return builtin->name;
}
return NULL;
}
extern const struct builtin_s g_builtins[];
extern const int g_builtin_count;
FAR const struct builtin_s *builtin_for_index(int index)
{
if (index < g_builtin_count)
{
return &g_builtins[index];
}
return NULL;
}
不难看出这段代码其实就是用来调用 “g_builtins”数组中的入口函数的。只是文件操作然人会有点糊涂,不知为何要通过文件操作去获取这些入口函数。所以现在我们需要了解的是 “builtin_loadbinary”函数在哪个地方调用。
接下来是 nsh初始化。
/****************************************************************************
* Name: nsh_initialize
*
* Description:
* This nterfaces is used to initialize the NuttShell (NSH).
* nsh_initialize() should be called one during application start-up prior
* to executing either nsh_consolemain() or nsh_telnetstart().
*
* Input Parameters:
* None
*
* Returned Value:
* None
*
****************************************************************************/
void nsh_initialize(void)
{
/* Mount the /etc filesystem */
(void)nsh_romfsetc();
/* Perform architecture-specific initialization (if available) */
(void)nsh_archinitialize();
/* Bring up the network */
(void)nsh_netinit();
}
通过注释我们知道,有两种方式可以用来执行 nsh: console和 telnet。 console使用串口通讯, telnet通过网络通讯。我们不使用网络所以不考了 telnet。
在该初始化函数里边我们只需关心第一个 nsh_romfsetc,用来挂载文件系统。
int nsh_romfsetc(void)
{
int ret;
/* Create a ROM disk for the /etc filesystem */
ret = romdisk_register(CONFIG_NSH_ROMFSDEVNO, romfs_img,
NSECTORS(romfs_img_len), CONFIG_NSH_ROMFSSECTSIZE);
if (ret < 0)
{
dbg("nsh: romdisk_register failed: %d\n", -ret);
return ERROR;
}
/* Mount the file system */
vdbg("Mounting ROMFS filesystem at target=%s with source=%s\n",
CONFIG_NSH_ROMFSMOUNTPT, MOUNT_DEVNAME);
ret = mount(MOUNT_DEVNAME, CONFIG_NSH_ROMFSMOUNTPT, "romfs", MS_RDONLY, NULL);
if (ret < 0)
{
dbg("nsh: mount(%s,%s,romfs) failed: %d\n",
MOUNT_DEVNAME, CONFIG_NSH_ROMFSMOUNTPT, errno);
return ERROR;
}
return OK;
}
之前我一直都以为 romfs是根文件系统,但这里只是将 romfs挂载到 /etc目录,所以并不是根文件系统,只有 /etc的内容。而根文件系统应该是存在与内存即 sram,所以 IO板正常运行。
那么 “nsh_consolemain”就是 nsh_main中我们最后需要去看的函数。
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;
}
基本上,可以以其调用的子函数将该函数划分成几个过程:nsh_newconsole、nsh_initscript和 nsh_session。
PX4 FMU启动流程 2. 一、 nsh_newconsole
这是一个 new函数。用来创建 stdio接口。
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;
}
其实也就是填充了结构 “struct console_stdio_s”。这些接口基本上都可以通过函数名大致了解其作用。除了函数指针外还有两个比较特殊的指针:输出流。
PX4 FMU启动流程 2. 二、 nsh_initscript
/****************************************************************************
* Name: nsh_initscript
*
* Description:
* Attempt to execute the configured initialization script. This script
* should be executed once when NSH starts. nsh_initscript is idempotent
* and may, however, be called multiple times (the script will be executed
* once.
*
****************************************************************************/
int nsh_initscript(FAR struct nsh_vtbl_s *vtbl)
{
static bool initialized;
bool already;
int ret = OK;
/* Atomic test and set of the initialized flag */
sched_lock();
already = initialized;
initialized = true;
sched_unlock();
/* If we have not already executed the init script, then do so now */
if (!already)
{
ret = nsh_script(vtbl, "init", NSH_INITPATH); // /etc/init.d/rcS
}
return ret;
}
#ifndef CONFIG_NSH_ROMFSMOUNTPT
#define CONFIG_NSH_ROMFSMOUNTPT "/etc"
#endif
#ifndef CONFIG_NSH_INITSCRIPT
#define CONFIG_NSH_INITSCRIPT "init.d/rcS"
#endif
#undef NSH_INITPATH
#define NSH_INITPATH CONFIG_NSH_ROMFSMOUNTPT "/" CONFIG_NSH_INITSCRIPT
我们很容易得出完整的路径是: “/etc/init.d/rcS”,这是一个脚本,在系统启动的时候需要调用的一个脚本,并且由 nsh进行解析。我们到不妨稍微看看这些脚本。
但其实我们根本就不用到: “ROMFS_ROOT”目录下去查看这些脚本,去 Bulid目录下即可查看,我们可以找到一个 “romfs_scratch”目录。
set MODE autostart
set USB autoconnect
if rgbled start
then
set HAVE_RGBLED 1
rgbled rgb 16 16 16
else
set HAVE_RGBLED 0
fi
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
set HAVE_MICROSD 1
tone_alarm start
else
echo "[init] no microSD card found"
set HAVE_MICROSD 0
tone_alarm 2
if [ $HAVE_RGBLED == 1 ]
then
rgbled rgb 16 0 0
fi
fi
if [ -f /fs/microsd/etc/rc ]
then
echo "[init] reading /fs/microsd/etc/rc"
sh /fs/microsd/etc/rc
fi
if [ -f /fs/microsd/etc/rc.txt ]
then
echo "[init] reading /fs/microsd/etc/rc.txt"
sh /fs/microsd/etc/rc.txt
fi
if [ $USB != autoconnect ]
then
echo "[init] not connecting USB"
else
if sercon
then
echo "[init] USB interface connected"
else
echo "[init] No USB connected"
fi
fi
if [ -f /etc/init.d/rc.APM -a $HAVE_MICROSD == 1 -a ! -f /fs/microsd/APM/nostart ]
then
echo Running rc.APM
sh /etc/init.d/rc.APM
else
nshterm /dev/ttyACM0 &
fi
这其中有些是 nsh内嵌的命令,有些是 “g_builtins”数组中定义的命令。
从脚本中我们看到,先是设置 LED,接这挂载 SD卡。 SD卡挂载上来之后就执行 SD卡中的脚本。最后再通过 “/etc/init.d/rc.APM”脚本去初始化飞控。
int nsh_script(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd,
FAR const char *path)
{
char *fullpath;
FILE *stream;
char *buffer;
char *pret;
int ret = ERROR;
/* The path to the script may be relative to the current working directory */
fullpath = nsh_getfullpath(vtbl, path);// /etc/init.d/rcS
if (!fullpath)
{
return ERROR;
}
/* Get a reference to the common input buffer */
buffer = nsh_linebuffer(vtbl);
if (buffer)
{
/* Open the file containing the script */
stream = fopen(fullpath, "r");
if (!stream)
{
nsh_output(vtbl, g_fmtcmdfailed, cmd, "fopen", NSH_ERRNO);
nsh_freefullpath(fullpath);
return ERROR;
}
/* Loop, processing each command line in the script file (or
* until an error occurs)
*/
do
{
/* Get the next line of input from the file */
fflush(stdout);
pret = fgets(buffer, CONFIG_NSH_LINELEN, stream);
if (pret)
{
/* Parse process the command. NOTE: this is recursive...
* we got to cmd_sh via a call to nsh_parse. So some
* considerable amount of stack may be used.
*/
ret = nsh_parse(vtbl, buffer);
}
}
while (pret && ret == OK);
fclose(stream);
}
nsh_freefullpath(fullpath);
return ret;
}
这其中调用了一个核心的函数是 “nsh_parse”,该函数用于解析 nsh脚本,而且是按行进行解析的。而前面只是获取脚本的完整路径跟 buffer,然后用文件操作函数打开脚本,再接着就是按行读入并解析。
nsh_parse函数比较长,五六百行代码。我们也就不可能把这些代码放到这里来看。我们在这里看流程关心的主要是该函数通过哪个函数把流程继续走下去的,这个函数就是:nsh_builtin。
/* Does this command correspond to a builtin command?
* nsh_builtin() returns:
*
* -1 (ERROR) if the application task corresponding to 'argv[0]' could not
* be started (possibly because it doesn not exist).
* 0 (OK) if the application task corresponding to 'argv[0]' was
* and successfully started. If CONFIG_SCHED_WAITPID is
* defined, this return value also indicates that the
* application returned successful status (EXIT_SUCCESS)
* 1 If CONFIG_SCHED_WAITPID is defined, then this return value
* indicates that the application task was spawned successfully
* but returned failure exit status.
*
* Note the priority if not effected by nice-ness.
*/
#if defined(CONFIG_NSH_BUILTIN_APPS) && (!defined(CONFIG_NSH_FILE_APPS) || !defined(CONFIG_FS_BINFS))
ret = nsh_builtin(vtbl, argv[0], argv, redirfile, oflags);
if (ret >= 0)
{
/* nsh_builtin() returned 0 or 1. This means that the builtin
* command was successfully started (although it may not have ran
* successfully). So certainly it is not an NSH command.
*/
/* Free the redirected output file path */
if (redirfile)
{
nsh_freefullpath(redirfile);
}
/* Save the result: success if 0; failure if 1 */
#ifndef CONFIG_DISABLE_ENVIRON
if (cmdline_exp != NULL)
free(cmdline_exp);
#endif
return nsh_saveresult(vtbl, ret != OK);
}
/* No, not a built in command (or, at least, we were unable to start a
* builtin command of that name). Treat it like an NSH command.
*/
#endif
从注释中我们知道返回值为 0的时候表示应用成功启动。当然注释并不能够告诉我们太多的细节。
熟悉 main函数定义形式的都知道 argv数组中 argv[0]所表示的意义。 argv数组是按下面的方式构造的:
/* Parse out the command at the beginning of the line */
saveptr = cmdline;
cmd = nsh_argument(vtbl, &saveptr);
/* Handler if-then-else-fi */
#ifndef CONFIG_NSH_DISABLESCRIPT
if (nsh_ifthenelse(vtbl, &cmd, &saveptr) != 0)
{
goto errout;
}
#endif
/* Handle nice */
#ifndef CONFIG_NSH_DISABLEBG
if (nsh_nice(vtbl, &cmd, &saveptr) != 0)
{
goto errout;
}
#endif
/* Check if any command was provided -OR- if command processing is
* currently disabled.
*/
#ifndef CONFIG_NSH_DISABLESCRIPT
if (!cmd || !nsh_cmdenabled(vtbl))
#else
if (!cmd)
#endif
{
/* An empty line is not an error and an unprocessed command cannot
* generate an error, but neither should they change the last
* command status.
*/
#ifndef CONFIG_DISABLE_ENVIRON
if (cmdline_exp != NULL)
free(cmdline_exp);
#endif
return OK;
}
/* Parse all of the arguments following the command name. The form
* of argv is:
*
* argv[0]: The command name.
* argv[1]: The beginning of argument (up to CONFIG_NSH_MAXARGUMENTS)
* argv[argc-3]: Possibly '>' or '>>'
* argv[argc-2]: Possibly <file>
* argv[argc-1]: Possibly '&'
* argv[argc]: NULL terminating pointer
*
* Maximum size is CONFIG_NSH_MAXARGUMENTS+5
*/
argv[0] = cmd;
for (argc = 1; argc < MAX_ARGV_ENTRIES-1; argc++)
{
argv[argc] = nsh_argument(vtbl, &saveptr);
if (!argv[argc])
{
break;
}
}
argv[argc] = NULL;
/* Check if the command should run in background */
#ifndef CONFIG_NSH_DISABLEBG
if (argc > 1 && strcmp(argv[argc-1], "&") == 0)
{
vtbl->np.np_bg = true;
argv[argc-1] = NULL;
argc--;
}
#endif
这段代码跟注释都已经写得足够清楚了。
举个例子,加入命令行为:“mount -t vfat /dev/mmcsd0 /fs/microsd”,那么经过解析之后在 argv数组将是:
* argv[0]: mount
* argv[1]: -t
* argv[2]: vfat
* argv[3]: /dev/mmcsd0
* argv[4]: /fs/microsd
* argv[5]: NULL terminating pointer
"&"表示把进程放到后台运行。其实熟悉 Linux的人一看到这个命令行就知道它会被解析成什么样。因为我们在 Linux应用编程中解析命令行参数的时候必须要了解这个,当然 Linux并没有要求所有的应用程序都提供解析命令行参数的功能。关于命令行参数解析的作用和意义这里就没必要说了,因为我们现在看的是流程,而不是教你怎么去解析命令行参数。
/****************************************************************************
* Name: nsh_builtin
*
* Description:
* Attempt to execute the application task whose name is 'cmd'
*
* Returned Value:
* <0 If exec_builtin() fails, then the negated errno value
* is returned.
* -1 (ERROR) if the application task corresponding to 'cmd' could not
* be started (possibly because it doesn not exist).
* 0 (OK) if the application task corresponding to 'cmd' was
* and successfully started. If CONFIG_SCHED_WAITPID is
* defined, this return value also indicates that the
* application returned successful status (EXIT_SUCCESS)
* 1 If CONFIG_SCHED_WAITPID is defined, then this return value
* indicates that the application task was spawned successfully
* but returned failure exit status.
*
****************************************************************************/
int nsh_builtin(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd,
FAR char **argv, FAR const char *redirfile, int oflags)
{
int ret = OK;
/* Lock the scheduler in an attempt to prevent the application from
* running until waitpid() has been called.
*/
sched_lock();
/* Try to find and execute the command within the list of builtin
* applications.
*/
ret = exec_builtin(cmd, (FAR char * const *)argv, redirfile, oflags);
if (ret >= 0)
{
}
sched_unlock();
/* If exec_builtin() or waitpid() failed, then return -1 (ERROR) with the
* errno value set appropriately.
*/
if (ret < 0)
{
return ERROR;
}
return ret;
}
容易看出来我把出错处理部分给去掉了。对于该函数的注释,其实要不要都无所谓,对英文不太懂的人像我就只是看一眼而已,更多的还是看代码。在 Linux应用编程中我们如果要调用一个 shell命令就会用到一个 “exec”函数。这这里作用是一样的。
int exec_builtin(FAR const char *appname, FAR char * const *argv,
FAR const char *redirfile, int oflags)
{
FAR const struct builtin_s *builtin;
posix_spawnattr_t attr;
posix_spawn_file_actions_t file_actions;
struct sched_param param;
pid_t pid;
int index;
int ret;
/* Verify that an application with this name exists */
index = builtin_isavail(appname);
if (index < 0)
{
ret = ENOENT;
goto errout_with_errno;
}
/* Get information about the builtin */
builtin = builtin_for_index(index);
if (builtin == NULL)
{
ret = ENOENT;
goto errout_with_errno;
}
/* Initialize attributes for task_spawn(). */
ret = posix_spawnattr_init(&attr);
if (ret != 0)
{
goto errout_with_errno;
}
ret = posix_spawn_file_actions_init(&file_actions);
if (ret != 0)
{
goto errout_with_attrs;
}
/* Set the correct task size and priority */
param.sched_priority = builtin->priority;
ret = posix_spawnattr_setschedparam(&attr, ¶m);
if (ret != 0)
{
goto errout_with_actions;
}
ret = task_spawnattr_setstacksize(&attr, builtin->stacksize);
if (ret != 0)
{
goto errout_with_actions;
}
/* If robin robin scheduling is enabled, then set the scheduling policy
* of the new task to SCHED_RR before it has a chance to run.
*/
#if CONFIG_RR_INTERVAL > 0
ret = posix_spawnattr_setschedpolicy(&attr, SCHED_RR);
if (ret != 0)
{
goto errout_with_actions;
}
ret = posix_spawnattr_setflags(&attr,
POSIX_SPAWN_SETSCHEDPARAM |
POSIX_SPAWN_SETSCHEDULER);
if (ret != 0)
{
goto errout_with_actions;
}
#else
ret = posix_spawnattr_setflags(&attr, POSIX_SPAWN_SETSCHEDPARAM);
if (ret != 0)
{
goto errout_with_actions;
}
#endif
/* Is output being redirected? */
if (redirfile)
{
/* Set up to close open redirfile and set to stdout (1) */
ret = posix_spawn_file_actions_addopen(&file_actions, 1,
redirfile, O_WRONLY, 0644);
if (ret != 0)
{
sdbg("ERROR: posix_spawn_file_actions_addopen failed: %d\n", ret);
goto errout_with_actions;
}
}
/* Start the built-in */
ret = task_spawn(&pid, builtin->name, builtin->main, &file_actions,
&attr, (argv) ? &argv[1] : (FAR char * const *)NULL,
(FAR char * const *)NULL);
if (ret != 0)
{
sdbg("ERROR: task_spawn failed: %d\n", ret);
goto errout_with_actions;
}
/* Free attibutes and file actions. Ignoring return values in the case
* of an error.
*/
/* Return the task ID of the new task if the task was sucessfully
* started. Otherwise, ret will be ERROR (and the errno value will
* be set appropriately).
*/
(void)posix_spawn_file_actions_destroy(&file_actions);
(void)posix_spawnattr_destroy(&attr);
return pid;
errout_with_actions:
(void)posix_spawn_file_actions_destroy(&file_actions);
errout_with_attrs:
(void)posix_spawnattr_destroy(&attr);
errout_with_errno:
set_errno(ret);
return ERROR;
}
记住这里有个变量: “struct builtin_s *builtin”,它是到数组 “g_builtins”中去取值。取到值之后就设置进程的属性,最后通过 “task_spawn”函数启动一个新的进程。
这样我们就看到了 Nuttx是如何启动我们的进程的。
突然发觉我们好像还漏掉了什么东西。因为我们知道 nsh命令不仅有我们写的命令,还有 Nuttx内嵌的命令,确切地所应该是 nsh内嵌的命令吧。那么这些命令有是如何被调用的呢?我们又到底遗漏了什么呢?我们又是否有一些比较好的方法去找到这些信息呢?
有一个稍微简单却繁杂的方法是:找到脚本中有系统提供的命令如 mount,然后我们可以匹配到这样一些信息:
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:302:int cmd_mount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:313: /* The mount command behaves differently if no parameters are provided */
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:318: return mount_show(vtbl, argv[0]);
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:322: /* Get the mount options. NOTE: getopt() is not thread safe nor re-entrant.
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:419: /* Perform the mount */
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:421: ret = mount(fullsource, fulltarget, filesystem, 0, NULL);
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:424: nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mount", NSH_ERRNO);
../PX4NuttX/apps/nshlib/nsh_mntcmds.c:443: * Name: cmd_nfsmount
看到 “cmd_mount”函数就有一种看到希望的感觉。因为该函数的命名很特殊,以 “cmd_”开头。我们写程序在给符号命名的时候通常会遵循一个默认的规则(记住这个不是规定更不是标准),就是望文生义。这说明该函数提供了 Nuttx系统的 “mount”命令。就这样,我找到了一个数组:
static const struct cmdmap_s g_cmdmap[] =
{
#if CONFIG_NFILE_DESCRIPTORS > 0
# ifndef CONFIG_NSH_DISABLE_CAT
{ "cat", cmd_cat, 2, CONFIG_NSH_MAXARGUMENTS, "<path> [<path> [<path> ...]]" },
# endif
#ifndef CONFIG_DISABLE_ENVIRON
# ifndef CONFIG_NSH_DISABLE_CD
{ "cd", cmd_cd, 1, 2, "[<dir-path>|-|~|..]" },
# endif
#endif
# ifndef CONFIG_NSH_DISABLE_CP
{ "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" },
# endif
#ifdef CONFIG_NET
# ifndef CONFIG_NSH_DISABLE_IFCONFIG
{ "ifconfig", cmd_ifconfig, 1, 11, "[nic_name [<ip-address>|dhcp]] [dr|gw|gateway <dr-address>] [netmask <net-mask>] [dns <dns-address>] [hw <hw-mac>]" },
# endif
# ifndef CONFIG_NSH_DISABLE_IFUPDOWN
{ "ifdown", cmd_ifdown, 2, 2, "<nic_name>" },
{ "ifup", cmd_ifup, 2, 2, "<nic_name>" },
# endif
#endif
#if CONFIG_NFILE_DESCRIPTORS > 0
# ifndef CONFIG_NSH_DISABLE_LS
{ "ls", cmd_ls, 1, 5, "[-lRs] <dir-path>" },
# endif
#endif
#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE)
# ifndef CONFIG_NSH_DISABLE_MKDIR
{ "mkdir", cmd_mkdir, 2, 2, "<path>" },
# endif
#endif
#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_READABLE)
# ifndef CONFIG_NSH_DISABLE_MOUNT
# ifdef CONFIG_NUTTX_KERNEL
{ "mount", cmd_mount, 5, 5, "-t <fstype> [<block-device>] <mount-point>" },
# else
{ "mount", cmd_mount, 1, 5, "[-t <fstype> [<block-device>] <mount-point>]" },
# endif
# endif
#endif
#if defined(CONFIG_NET) && defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \
!defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS)
# ifndef CONFIG_NSH_DISABLE_PING
{ "ping", cmd_ping, 2, 6, "[-c <count>] [-i <interval>] <ip-address>" },
# endif
#endif
#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON)
# ifndef CONFIG_NSH_DISABLE_PWD
{ "pwd", cmd_pwd, 1, 1, NULL },
# endif
#endif
{ NULL, NULL, 1, 1, NULL }
};
这个数组明显是被我裁过的,我只留下了 Linux中比较具有代表性的几个命令。其类型如下:
struct cmdmap_s
{
const char *cmd; /* Name of the command */
cmd_t handler; /* Function that handles the command */
uint8_t minargs; /* Minimum number of arguments (including command) */
uint8_t maxargs; /* Maximum number of arguments (including command) */
const char *usage; /* Usage instructions for 'help' command */
};
跟 “g_builtins”数组一样,命令匹配由一个专门的字符串完成。既然这个数组都找到了,那么现在的问题是这个数组在哪里使用了。
bitcraze@bitcraze-vm:~/apm/PX4Firmware$ grep -nr g_cmdmap ../PX4NuttX/apps/
../PX4NuttX/apps/examples/ftpc/ftpc_main.c:81:static const struct cmdmap_s g_cmdmap[] =
../PX4NuttX/apps/examples/ftpc/ftpc_main.c:120: for (ptr = g_cmdmap; ptr->cmd; ptr++)
../PX4NuttX/apps/examples/ftpc/ftpc_main.c:249: for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++)
../PX4NuttX/apps/nshlib/nsh_parse.c:97:#define NUM_CMDS ((sizeof(g_cmdmap)/sizeof(struct cmdmap_s)) - 1)
../PX4NuttX/apps/nshlib/nsh_parse.c:147:static const struct cmdmap_s g_cmdmap[] =
../PX4NuttX/apps/nshlib/nsh_parse.c:518: nsh_output(vtbl, "%-12s", g_cmdmap[k].cmd);
../PX4NuttX/apps/nshlib/nsh_parse.c:581: for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++)
../PX4NuttX/apps/nshlib/nsh_parse.c:611: for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++)
../PX4NuttX/apps/nshlib/nsh_parse.c:777: for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++)
bitcraze@bitcraze-vm:~/apm/PX4Firmware$
这个时候我才发现自己刚才匹配 “mount”是多么幸运。因为加入我匹配的是 “ls”命令,我可能就会找到 “ftpc_main.c”里边去,毕竟在那么多的匹配结果里边人是很容易犯错的。
经过帅选,最后确定了函数 “nsh_execute”。
static int nsh_execute(FAR struct nsh_vtbl_s *vtbl, int argc, char *argv[])
{
const struct cmdmap_s *cmdmap;
const char *cmd;
cmd_t handler = cmd_unrecognized;
int ret;
/* The form of argv is:
*
* argv[0]: The command name. This is argv[0] when the arguments
* are, finally, received by the command vtblr
* argv[1]: The beginning of argument (up to CONFIG_NSH_MAXARGUMENTS)
* argv[argc]: NULL terminating pointer
*/
cmd = argv[0];
/* See if the command is one that we understand */
for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++)
{
if (strcmp(cmdmap->cmd, cmd) == 0)
{
/* Check if a valid number of arguments was provided. We
* do this simple, imperfect checking here so that it does
* not have to be performed in each command.
*/
if (argc < cmdmap->minargs)
{
/* Fewer than the minimum number were provided */
nsh_output(vtbl, g_fmtargrequired, cmd);
return ERROR;
}
else if (argc > cmdmap->maxargs)
{
/* More than the maximum number were provided */
nsh_output(vtbl, g_fmttoomanyargs, cmd);
return ERROR;
}
else
{
/* A valid number of arguments were provided (this does
* not mean they are right).
*/
handler = cmdmap->handler;
break;
}
}
}
ret = handler(vtbl, argc, argv);
return ret;
}
所以我们看到,最后是通过 “cmdmap->handler”找到了命令对应的函数,并通过指针 “handler”进行调用。而函数 “nsh_execute”其实有两种方式进行调用:线程和函数调用。当然这都发生在 nsh解析函数 “nsh_parse”里边。
/* Handle the case where the command is executed in background.
* However is app is to be started as builtin new process will
* be created anyway, so skip this step.
*/
#ifndef CONFIG_NSH_DISABLEBG
if (vtbl->np.np_bg)
{
struct sched_param param;
struct nsh_vtbl_s *bkgvtbl;
struct cmdarg_s *args;
pthread_attr_t attr;
pthread_t thread;
/* Get a cloned copy of the vtbl with reference count=1.
* after the command has been processed, the nsh_release() call
* at the end of nsh_child() will destroy the clone.
*/
/* Set up the thread attributes */
(void)pthread_attr_init(&attr);
(void)pthread_attr_setschedpolicy(&attr, SCHED_NSH);
(void)pthread_attr_setschedparam(&attr, ¶m);
/* Execute the command as a separate thread at the appropriate priority */
ret = pthread_create(&thread, &attr, nsh_child, (pthread_addr_t)args);
if (ret != 0)
{
nsh_output(vtbl, g_fmtcmdfailed, cmd, "pthread_create", NSH_ERRNO_OF(ret));
nsh_releaseargs(args);
nsh_release(bkgvtbl);
goto errout;
}
/* Detach from the pthread since we are not going to join with it.
* Otherwise, we would have a memory leak.
*/
(void)pthread_detach(thread);
nsh_output(vtbl, "%s [%d:%d]\n", cmd, thread, param.sched_priority);
}
else
#endif
{
uint8_t save[SAVE_SIZE];
ret = nsh_execute(vtbl, argc, argv);
}
#ifndef CONFIG_NSH_DISABLEBG
static pthread_addr_t nsh_child(pthread_addr_t arg)
{
struct cmdarg_s *carg = (struct cmdarg_s *)arg;
int ret;
dbg("BG %s\n", carg->argv[0]);
/* Execute the specified command on the child thread */
ret = nsh_execute(carg->vtbl, carg->argc, carg->argv);
/* Released the cloned arguments */
dbg("BG %s complete\n", carg->argv[0]);
nsh_releaseargs(carg);
return (void*)ret;
}
#endif
代码比较多,我自然又忍不住删去了一些,只留下了基本的骨架。
所以,如果我们要把系统命令放在后台运行就定义宏 “CONFIG_NSH_DISABLEBG”,否则就是直接调用。而这些过程,都是发生在调用 “nsh_builtin”函数之后。
回过头来一看才发现函数 “nsh_parse”都已经看了个七七八八了。
PX4 FMU启动流程 2. 三、nsh_session
有了前面的铺垫,这时候我们看 “nsh_session”函数就简单很多了。
int nsh_session(FAR struct console_stdio_s *pstate)
{
int ret;
DEBUGASSERT(pstate);
/* Present a greeting */
fputs(g_nshgreeting, pstate->cn_outstream);
fflush(pstate->cn_outstream);
/* Execute the login script */
#ifdef CONFIG_NSH_ROMFSRC
(void)nsh_loginscript(&pstate->cn_vtbl);
#endif
/* Then enter the command line parsing loop */
for (;;)
{
/* For the case of debugging the USB console... dump collected USB trace data */
#ifdef CONFIG_NSH_USBDEV_TRACE
nsh_usbtrace();
#endif
/* Display the prompt string */
fputs(g_nshprompt, pstate->cn_outstream);
fflush(pstate->cn_outstream);
/* Get the next line of input. readline() returns EOF on end-of-file
* or any read failure.
*/
ret = readline(pstate->cn_line, CONFIG_NSH_LINELEN,
INSTREAM(pstate), OUTSTREAM(pstate));
if (ret != EOF)
{
/* Parse process the command */
(void)nsh_parse(&pstate->cn_vtbl, pstate->cn_line);
fflush(pstate->cn_outstream);
}
/* Readline normally returns the number of characters read,
* but will return EOF on end of file or if an error occurs.
* EOF will cause the session to terminate.
*/
else
{
fprintf(pstate->cn_outstream, g_fmtcmdfailed, "nsh_session",
"readline", NSH_ERRNO_OF(-ret));
return ret == 0 ? EXIT_SUCCESS : EXIT_FAILURE;
}
}
/* We do not get here, but this is necessary to keep some compilers happy.
* But others will complain that this code is not reachable.
*/
return EXIT_SUCCESS;
}
跟 “nsh_script”函数还是挺相似的。不同在于这里的命令行不是来自脚本文件,而事实来自 outstream,是什么呢?我们可以在 “nsh_newconsole”函数中找到。其实就是与用户交互的命令行终端。我们用上位机连上飞控的时候可以在一个窗口中用命令与飞控交互,这部分工作就是由这里完成的。
应该来说,从中断读数据是不会读到文件结束标志的,所以该循环不会跳出。
PX4 FMU启动流程 3. 启动脚本
这里所说的脚本当然不是我们前面说到的编辑脚本,而是由 nsh进行解析的脚本,如 rcS。
set MODE autostart
set USB autoconnect
if rgbled start
then
set HAVE_RGBLED 1
rgbled rgb 16 16 16
else
set HAVE_RGBLED 0
fi
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
set HAVE_MICROSD 1
tone_alarm start
else
echo "[init] no microSD card found"
set HAVE_MICROSD 0
tone_alarm 2
if [ $HAVE_RGBLED == 1 ]
then
rgbled rgb 16 0 0
fi
fi
if [ -f /fs/microsd/etc/rc ]
then
echo "[init] reading /fs/microsd/etc/rc"
sh /fs/microsd/etc/rc
fi
if [ -f /fs/microsd/etc/rc.txt ]
then
echo "[init] reading /fs/microsd/etc/rc.txt"
sh /fs/microsd/etc/rc.txt
fi
if [ $USB != autoconnect ]
then
echo "[init] not connecting USB"
else
if sercon
then
echo "[init] USB interface connected"
else
echo "[init] No USB connected"
fi
fi
if [ -f /etc/init.d/rc.APM -a $HAVE_MICROSD == 1 -a ! -f /fs/microsd/APM/nostart ]
then
echo Running rc.APM
sh /etc/init.d/rc.APM
else
nshterm /dev/ttyACM0 &
fi
其实我们完全把它当作一个 Linux脚本来阅读就可以了。
MODE跟 USB应该是环境变量,在 Linux中也是用 set去给一个环境变量赋值的。然后去启动 “rgbled”,这是由 “Firmware”提供的。我们很容易猜到这是用来控制那个高亮的 LED灯的。然后才是挂载 SD卡,文件系统为 vfat,挂载目录为 /fs/microsd。 SD卡挂载上来之后就去执行 SD卡中的脚本 “/fs/microsd/etc/rc”,所以我们就知道 sh是用来执行 nsh脚本的一个命令。应该来说 “/fs/microsd/etc/rc.txt”也是一个脚本,要不然也不会使用 sh命令了。 sercon你需要去看下源码才知道这是用来初始化 USB串口的。而最后则是通过调用 /etc/init.d/rc.APM脚本来真正初始化飞控。
脚本 /etc/init.d/rc.APM有近三百行,不过别误会,我可没这闲情逸致去数它到底多少行。
所以现在我们需要将脚本进行拆分。那怎么拆呢?其实我也头疼,只能是凭感觉了。
set deviceA /dev/ttyACM0
if [ -f /fs/microsd/APM ]
then
echo "APM file found - renaming"
mv /fs/microsd/APM /fs/microsd/APM.old
fi
if [ -f /fs/microsd/APM/nostart ]
then
echo "APM/nostart found - skipping APM startup"
sh /etc/init.d/rc.error
fi
if [ -f /bin/reboot ]
then
echo "binfs already mounted"
else
echo "Mounting binfs"
if mount -t binfs /dev/null /bin
then
echo "binfs mounted OK"
else
sh /etc/init.d/rc.error
fi
fi
set sketch NONE
if rm /fs/microsd/APM/boot.log
then
echo "removed old boot.log"
fi
set logfile /fs/microsd/APM/BOOT.LOG
if [ ! -f /bin/ArduPilot ]
then
echo "/bin/ardupilot not found"
sh /etc/init.d/rc.error
fi
if mkdir /fs/microsd/APM > /dev/null
then
echo "Created APM directory"
fi
这部分是我划出来的第一部分。先设置串口,然后一堆跟 SD卡相关的命令。其实我们将一张空的 SD卡放进去也是可以正常工作的。可能这里最让人感兴趣的就是 “ArduPilot”了,因为 APM工程就叫 “ArduPilot”。在 g_builtins数组中也有这样一行信息:
{"ArduPilot", SCHED_PRIORITY_DEFAULT, 4096, ArduPilot_main},
也就是说 PX4将 APM原来的主程序设计成了 Nuttx中的一个命令。但其实我们在使用 Linux系统的时候,可执行文件其实就等同命令。这个命令跟其他命令还是有很大不同的,这个我们后面再讨论。
if [ -f /bin/lsm303d ]
then
echo "Detected FMUv2 board"
set BOARD FMUv2
else
echo "Detected FMUv1 board"
set BOARD FMUv1
fi
if [ $BOARD == FMUv1 ]
then
set deviceC /dev/ttyS2
if [ -f /fs/microsd/APM/AUXPWM.en ]
then
set deviceD /dev/null
else
set deviceD /dev/ttyS1
fi
else
set deviceC /dev/ttyS1
set deviceD /dev/ttyS2
fi
if uorb start
then
echo "uorb started OK"
else
sh /etc/init.d/rc.error
fi
从这里我们可以看到 FMUv1跟 FMUv2的差别在于 lsm303d这颗传感器。通过这颗传感器我们可以用肉眼进行区分。当然它们所使用的串口不同这个可能就无法直接看到了。
我看了下 uorb的源码,源码中有这样一条注释:“Start/load the driver”,我没太搞懂这里 “driver”是什么,只是它用到了一个变量:PX4IO *g_dev = nullptr;,由此也只能确定这是跟 IO板相关的,更具体的就不知道了。
if [ -f /fs/microsd/APM/mkblctrl ]
then
echo "Setting up mkblctrl driver"
echo "Setting up mkblctrl driver" >> $logfile
mkblctrl -d /dev/pwm_output
fi
if [ -f /fs/microsd/APM/mkblctrl_+ ]
then
echo "Setting up mkblctrl driver +"
echo "Setting up mkblctrl driver +" >> $logfile
mkblctrl -mkmode + -d /dev/pwm_output
fi
if [ -f /fs/microsd/APM/mkblctrl_x ]
then
echo "Setting up mkblctrl driver x"
echo "Setting up mkblctrl driver x" >> $logfile
mkblctrl -mkmode x -d /dev/pwm_output
fi
这一段我想应该是比较好理解的,就算我们不去看这个命令,因为这里是在设置机型。
set HAVE_PX4IO false
if px4io start norc
then
set HAVE_PX4IO true
else
echo Loading /etc/px4io/px4io.bin
tone_alarm MBABGP
if px4io update /etc/px4io/px4io.bin
then
echo "upgraded PX4IO firmware OK"
tone_alarm MSPAA
else
echo "Failed to upgrade PX4IO firmware"
tone_alarm MNGGG
fi
sleep 1
if px4io start norc
then
set HAVE_PX4IO true
tone_alarm start
fi
fi
if [ $HAVE_PX4IO == true ]
then
echo "PX4IO board OK"
if px4io checkcrc /etc/px4io/px4io.bin
then
echo "PX4IO CRC OK"
else
echo "PX4IO CRC failure"
echo "PX4IO CRC failure" >> $logfile
tone_alarm MBABGP
if px4io forceupdate 14662 /etc/px4io/px4io.bin
then
sleep 1
if px4io start norc
then
echo "PX4IO restart OK"
echo "PX4IO restart OK" >> $logfile
tone_alarm MSPAA
else
echo "PX4IO restart failed"
echo "PX4IO restart failed" >> $logfile
tone_alarm MNGGG
sh /etc/init.d/rc.error
fi
else
echo "PX4IO update failed"
echo "PX4IO update failed" >> $logfile
tone_alarm MNGGG
fi
fi
else
echo "No PX4IO board found"
echo "No PX4IO board found" >> $logfile
if [ $BOARD == FMUv2 ]
then
sh /etc/init.d/rc.error
fi
fi
这里看似很长的一段脚本,其实质做了一件事情:更新 IO板的固件。这么长的脚本处理了两种情况:一、IO板没有固件;二,IO板有固件但不是最新的。更新固件是通过 px4io命令来完成的,如果我们想知道固件是如何更新的可以去阅读该命令的源码。固件更新其实我们在分析 IO板的启动的时候已经知道是通过 CRC校验判断是否需要更新的。
if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
then
echo "Setting FMU mode_serial"
fmu mode_serial
else
echo "Setting FMU mode_pwm"
fmu mode_pwm
fi
我粗略看了下 fmu的源码,没看明白这一段是干嘛的。
echo "Starting APM sensors"
if ms5611 start
then
echo "ms5611 started OK"
else
sh /etc/init.d/rc.error
fi
if adc start
then
echo "adc started OK"
else
sh /etc/init.d/rc.error
fi
if [ $BOARD == FMUv1 ]
then
echo "Starting FMUv1 sensors"
if hmc5883 start
then
echo "hmc5883 started OK"
if hmc5883 calibrate
then
echo "hmc5883 calibrate OK"
else
echo "hmc5883 calibrate failed"
echo "hmc5883 calibrate failed" >> $logfile
tone_alarm MSBBB
fi
else
echo "hmc5883 start failed"
echo "hmc5883 start failed" >> $logfile
sh /etc/init.d/rc.error
fi
if mpu6000 start
then
echo "mpu6000 started OK"
else
sh /etc/init.d/rc.error
fi
if l3gd20 start
then
echo "l3gd20 started OK"
else
echo "No l3gd20"
echo "No l3gd20" >> $logfile
fi
else
echo "Starting FMUv2 sensors"
if hmc5883 -C -X start
then
echo "Have external hmc5883"
else
echo "No external hmc5883"
fi
if hmc5883 -C -I -R 4 start
then
echo "Have internal hmc5883"
else
echo "No internal hmc5883"
fi
if mpu6000 -X -R 4 start
then
echo "Found MPU6000 external"
set HAVE_FMUV3 true
else
echo "No MPU6000 external"
set HAVE_FMUV3 false
fi
if [ $HAVE_FMUV3 == true ]
then
if mpu6000 -R 14 start
then
echo "Found MPU6000 internal"
else
echo "No MPU6000"
echo "No MPU6000" >> $logfile
sh /etc/init.d/rc.error
fi
if l3gd20 -X -R 4 start
then
echo "l3gd20 external started OK"
else
echo "No l3gd20"
sh /etc/init.d/rc.error
fi
if lsm303d -X -R 6 start
then
echo "lsm303d external started OK"
else
echo "No lsm303d"
sh /etc/init.d/rc.error
fi
else
if mpu6000 start
then
echo "Found MPU6000"
else
echo "No MPU6000"
echo "No MPU6000" >> $logfile
fi
if l3gd20 start
then
echo "l3gd20 started OK"
else
sh /etc/init.d/rc.error
fi
if lsm303d start
then
echo "lsm303d started OK"
else
sh /etc/init.d/rc.error
fi
fi
fi
if ets_airspeed start
then
echo "Found ETS airspeed sensor"
fi
if meas_airspeed start
then
echo "Found MEAS airspeed sensor"
fi
if ll40ls start
then
echo "Found ll40ls sensor"
fi
if mb12xx start
then
echo "Found mb12xx sensor"
fi
从 echo那一句很容易看出这里是在启动传感器。当然,我暂时是不会去研究这些传感器是干嘛用的,只以看流程为主。从这段脚本我们也可以看出 V1跟 V2的一些区别,即 V1是内置罗盘,并且上电校准。但是 V2可以使用外置罗盘,而且上电过程是不校准的。至于陀螺为什么会有内部和外部的区别这个我没想通,也没有找到相关资料,这或许就要到源码中去寻找答案了。而源码肯定是要去看的。
echo "Trying PX4IO board"
if mtd start /fs/mtd
then
echo "started mtd driver OK"
else
echo "failed to start mtd driver"
echo "failed to start mtd driver" >> $logfile
sh /etc/init.d/rc.error
fi
if mtd readtest /fs/mtd
then
echo "mtd readtest OK"
else
echo "failed to read mtd"
echo "failed to read mtd" >> $logfile
sh /etc/init.d/rc.error
fi
if [ $BOARD == FMUv2 ]
then
if mtd rwtest /fs/mtd
then
echo "mtd rwtest OK"
else
echo "failed to test mtd"
echo "failed to test mtd" >> $logfile
sh /etc/init.d/rc.error
fi
fi
echo Starting ArduPilot $deviceA $deviceC $deviceD
if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
then
echo ArduPilot started OK
else
sh /etc/init.d/rc.error
fi
echo "rc.APM finished"
这部分脚本是对 mtd的操作,根据对 Linux的了解,我觉得这里应该跟存储设备相关。虽然没有 mount操作,但我仍然觉得这里可能是挂载存储设备,具体的仍然要到源码中去找寻答案。
最后还有一个人脚本: rc.error,在出错的时候我们经常看到这个脚本。
echo "Error in startup"
tone_alarm MNCC
if [ $HAVE_RGBLED == 1 ]
then
rgbled rgb 16 0 0
fi
nshterm /dev/ttyACM0 &
sleep 1
nshterm /dev/ttyS0 &
sleep 1
exit
也就是出错处理。当出错的时候输出一些信息,并且终止初始化。
PX4 IO板启动流程
1. IO板启动
在前面分析脚本的时候我们知道 IO板的源码会在 “config_px4io-v2_default.mk”中指定。
#
# Makefile for the px4iov2_default configuration
#
#
# Board support modules
#
MODULES += drivers/stm32
MODULES += drivers/boards/px4io-v2
MODULES += modules/px4iofirmware
那我们就不妨到这些目录下去看看都有些什么文件。
bitcraze@bitcraze-vm:~/apm/PX4Firmware/src$ ls drivers/stm32/
adc drv_hrt.c drv_pwm_servo.c drv_pwm_servo.h module.mk tone_alarm
bitcraze@bitcraze-vm:~/apm/PX4Firmware/src$ ls drivers/boards/px4io-v2/
board_config.h module.mk px4iov2_init.c px4iov2_pwm_servo.c
bitcraze@bitcraze-vm:~/apm/PX4Firmware/src$ ls modules/px4iofirmware/
adc.c dsm.c mixer.cpp protocol.h px4io.h safety.c serial.c
controls.c i2c.c module.mk px4io.c registers.c sbus.c
bitcraze@bitcraze-vm:~/apm/PX4Firmware/src$
所以源码不是很多,除非是所使用的 lib里边有大动作,否则很难想像这里边有很复杂的算法。只是从PCB上看, SBUS和主电机都由 IO板进行控制。
在 “drivers/boards/px4io-v2”目录下, “module.mk”文件的内容为:
#
# Board-specific startup code for the PX4IOv2
#
SRCS = px4iov2_init.c \
px4iov2_pwm_servo.c
在该木兰下其实也就只有这两个源文件。而头文件 “board_config.h”我们如果去阅读其内容就会发现里面基本上是 IO板的硬件资源。而源文件 “px4iov2_init.c”从名称上看就是一段初始化代码,其中也确实只有一个函数:
__EXPORT void stm32_boardinitialize(void)
{
/* configure GPIOs */
/* LEDS - default to off */
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_BTN_SAFETY);
/* spektrum power enable is active high - enable it by default */
stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
/* RSSI inputs */
stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
stm32_configgpio(GPIO_ADC_RSSI);
/* servo rail voltage */
stm32_configgpio(GPIO_ADC_VSERVO);
stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
stm32_configgpio(GPIO_SBUS_OUTPUT);
/* sbus output enable is active low - disable it by default */
stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
stm32_configgpio(GPIO_SBUS_OENABLE);
stm32_configgpio(GPIO_PPM); /* xxx alternate function */
stm32_gpiowrite(GPIO_PWM1, false);
stm32_configgpio(GPIO_PWM1);
stm32_gpiowrite(GPIO_PWM2, false);
stm32_configgpio(GPIO_PWM2);
stm32_gpiowrite(GPIO_PWM3, false);
stm32_configgpio(GPIO_PWM3);
stm32_gpiowrite(GPIO_PWM4, false);
stm32_configgpio(GPIO_PWM4);
stm32_gpiowrite(GPIO_PWM5, false);
stm32_configgpio(GPIO_PWM5);
stm32_gpiowrite(GPIO_PWM6, false);
stm32_configgpio(GPIO_PWM6);
stm32_gpiowrite(GPIO_PWM7, false);
stm32_configgpio(GPIO_PWM7);
stm32_gpiowrite(GPIO_PWM8, false);
stm32_configgpio(GPIO_PWM8);
}
确切地说,这其实是一个入口函数,如果进行匹配,我们会找到这样一段信息:
../../PX4NuttX/nuttx/arch/arm/src/stm32/stm32_start.c:254: stm32_boardinitialize();
该源文件的内容如下:
/****************************************************************************
* Name: _start
*
* Description:
* This is the reset entry point.
*
****************************************************************************/
void __start(void)
{
const uint32_t *src;
uint32_t *dest;
#ifdef CONFIG_ARMV7M_STACKCHECK
/* Set the stack limit before we attempt to call any functions */
__asm__ volatile ("sub r10, sp, %0" : : "r" (CONFIG_IDLETHREAD_STACKSIZE - 64) : );
#endif
/* Configure the uart so that we can get debug output as soon as possible */
stm32_clockconfig();
stm32_fpuconfig();
stm32_lowsetup();
stm32_gpioinit();
showprogress('A');
/* Clear .bss. We'll do this inline (vs. calling memset) just to be
* certain that there are no issues with the state of global variables.
*/
for (dest = &_sbss; dest < &_ebss; )
{
*dest++ = 0;
}
showprogress('B');
/* Move the intialized data section from his temporary holding spot in
* FLASH into the correct place in SRAM. The correct place in SRAM is
* give by _sdata and _edata. The temporary location is in FLASH at the
* end of all of the other read-only data (.text, .rodata) at _eronly.
*/
for (src = &_eronly, dest = &_sdata; dest < &_edata; )
{
*dest++ = *src++;
}
showprogress('C');
/* Perform early serial initialization */
#ifdef USE_EARLYSERIALINIT
up_earlyserialinit();
#endif
showprogress('D');
/* For the case of the separate user-/kernel-space build, perform whatever
* platform specific initialization of the user memory is required.
* Normally this just means initializing the user space .data and .bss
* segments.
*/
#ifdef CONFIG_NUTTX_KERNEL
stm32_userspace();
showprogress('E');
#endif
/* Initialize onboard resources */
stm32_boardinitialize();
showprogress('F');
/* Then start NuttX */
showprogress('\r');
showprogress('\n');
os_start();
/* Shoulnd't get here */
for(;;);
}
__start函数是 Nuttx系统的入口函数。
可能有些人就会有疑问了,C语言的入口函数不应该是 main函数的吗?确实,我们都说 C语言的入口是 main函数,而且我们也都是这么做的。但是请记住, main函数本质上也是一个函数,没有区别,只是在我们不手动指定的情况下编译器默认入口函数为 main函数,但其实入口是可以手动进行指定的。这里用的是链接脚本。编译过的源码在 “PX4Firmware/Build”目录下会有一个 “px4io-v2_default.build”目录。你可以在 “PX4Firmware/Build/px4io-v2_default.build/nuttx-export/build”目录下找到 “ld.script”文件,内容如下:
/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
* 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH,
* FLASH memory is aliased to address 0x0000:0000 where the code expects to
* begin execution by jumping to the entry point in the 0x0800:0000 address
* range.
*/
MEMORY
{
flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
}
OUTPUT_ARCH(arm)
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
EXTERN(_vectors) /* force the vectors to be included in the output */
/*
* Ensure that abort() is present in the final object. The exception handling
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
*/
EXTERN(abort)
SECTIONS
{
.text : {
_stext = ABSOLUTE(.);
*(.vectors)
*(.text .text.*)
*(.fixup)
*(.gnu.warning)
*(.rodata .rodata.*)
*(.gnu.linkonce.t.*)
*(.glue_7)
*(.glue_7t)
*(.got)
*(.gcc_except_table)
*(.gnu.linkonce.r.*)
_etext = ABSOLUTE(.);
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : {
*(.ARM.extab*)
} > flash
__exidx_start = ABSOLUTE(.);
.ARM.exidx : {
*(.ARM.exidx*)
} > flash
__exidx_end = ABSOLUTE(.);
_eronly = ABSOLUTE(.);
/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
.data : {
_sdata = ABSOLUTE(.);
*(.data .data.*)
*(.gnu.linkonce.d.*)
CONSTRUCTORS
_edata = ABSOLUTE(.);
} > sram AT > flash
.bss : {
_sbss = ABSOLUTE(.);
*(.bss .bss.*)
*(.gnu.linkonce.b.*)
*(COMMON)
_ebss = ABSOLUTE(.);
} > sram
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_info 0 : { *(.debug_info) }
.debug_line 0 : { *(.debug_line) }
.debug_pubnames 0 : { *(.debug_pubnames) }
.debug_aranges 0 : { *(.debug_aranges) }
}
我们知道 STM32的 Flash起始地址是 0x08000000,而且大小从来都没有听说做有 60K的。实际上这里用的是 64K芯片, 0x1000也正是 4K。稍微了解下 PX4都知道, PX4的两个芯片都是有 bootloader的,也就是说这 4K用来存放 bootloader。
同时我们也看到,链接脚本中有:“ENTRY(__start)”这样一段内容,也就是这个地方指定了程序的入口。
我们可以找到这样一个文件 “PX4NuttX/nuttx/arch/arm/src/stm32/stm32_vectors.S”,一看就知道里边定义了中断向量表。在链接脚本中我们看到,在程序开始的地方放的是中断向量表,那么这个中断向量表又有哪些内容呢?
#define IDLE_STACK (_ebss+CONFIG_IDLETHREAD_STACKSIZE-4)
#define HEAP_BASE (_ebss+CONFIG_IDLETHREAD_STACKSIZE)
/************************************************************************************
* Global Symbols
************************************************************************************/
.syntax unified
.thumb
.file "stm32_vectors.S"
/* Check if common ARMv7 interrupt vectoring is used (see arch/arm/src/armv7-m/up_vectors.S) */
#ifndef CONFIG_ARMV7M_CMNVECTOR
.globl __start
/************************************************************************************
* Macros
************************************************************************************/
/* On entry into an IRQ, the hardware automatically saves the xPSR, PC, LR, R12, R0-R3
* registers on the stack, then branches to an instantantiation of the following
* macro. This macro simply loads the IRQ number into R0, then jumps to the common
* IRQ handling logic.
*/
.macro HANDLER, label, irqno
.thumb_func
\label:
mov r0, #\irqno
b stm32_common
.endm
/************************************************************************************
* Vectors
************************************************************************************/
.section .vectors, "ax"
.code 16
.align 2
.globl stm32_vectors
.type stm32_vectors, function
stm32_vectors:
/* Processor Exceptions */
.word IDLE_STACK /* Vector 0: Reset stack pointer */
.word __start /* Vector 1: Reset vector */
.word stm32_nmi /* Vector 2: Non-Maskable Interrupt (NMI) */
.word stm32_hardfault /* Vector 3: Hard fault */
.word stm32_mpu /* Vector 4: Memory management (MPU) */
.word stm32_busfault /* Vector 5: Bus fault */
.word stm32_usagefault /* Vector 6: Usage fault */
.word stm32_reserved /* Vector 7: Reserved */
.word stm32_reserved /* Vector 8: Reserved */
.word stm32_reserved /* Vector 9: Reserved */
.word stm32_reserved /* Vector 10: Reserved */
.word stm32_svcall /* Vector 11: SVC call */
.word stm32_dbgmonitor /* Vector 12: Debug monitor */
.word stm32_reserved /* Vector 13: Reserved */
.word stm32_pendsv /* Vector 14: Pendable system service request */
.word stm32_systick /* Vector 15: System tick */
/* External Interrupts */
阅读过 keil中 STM32启动代码的应该大体上都能看懂这段代码。在 keil中最后是调用了一个 “__main”的符号进入 main函数的,其实就是 C语言函数汇编之后的符号。这里其实都一样,只是入口函数不再是 main函数而已。
这里定义了堆栈大小还有 “.vectors”段,即中断向量表。 bootloader引导的时候会进入该中断向量表的复位向量即 “__start”。之后就是初始化,知道调用我们的 stm32_boardinitialize函数并启动系统。而在我们的初始化中基本上都是对 IO进行配置。
那么现在的疑问是,当系统启动之后是如何调用我们的其他函数的?进程调用吗?这是操作系统的通用做法。而且这样做通常需要把我们自己的程序设计成操作系统中的一个进程。
如果我们去阅读 IO板的相关源码,我们会在 px4io.c文件中看到这样一个函数:
int user_start(int argc, char *argv[])
{
/* 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);
/* 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();
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* 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();
}
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)) {
struct mallinfo minfo = mallinfo();
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)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
user_start函数之所以能够吸引我主要基于下面三点:
一、函数的定义。 “int user_start(int argc, char *argv[])”是一个标准的 main函数形式。而 main函数通常作为程序的入口,这也就意味着这是一个入口函数。而且可以推测这是应用程序入口,因为至少目前在 Linux中不会这样去写一个驱动的入口, Nuttx是类 Linux系统,我自然确信不会这么做。那么既然是入口,那肯定有人去调用。
二、这里调用了一些初始化函数,也就是说 IO板在这里进行初始化。那我们就容易想到这实际上就相当于我们跑裸机时的 main函数。
三、主循环。我们跑裸机的时候一定会有一个主循环。不管这个循环是在 mian函数还是子函数中,但肯定会有。
这三点可以说确定了 user_start函数在 IO的地位。而且我们可以找到以下内容:
../PX4NuttX/nuttx/.config:320:CONFIG_USER_ENTRYPOINT="user_start"
“.config”是 Nuttx系统的配置文件,而 “CONFIG_USER_ENTRYPOINT”的命名已经明明白白地告诉我们这是一个入口,用户入口。最后我们可以匹配到下面的内容:
../PX4NuttX/nuttx/include/nuttx/init.h:71:EXTERN int CONFIG_USER_ENTRYPOINT(int argc,char *argv[]);
../PX4NuttX/nuttx/include/nuttx/config.h:89:#define CONFIG_USER_ENTRYPOINT user_start
../PX4NuttX/nuttx/include/nuttx/config.h:389: * with existing code,for builds whichdo not define CONFIG_USER_ENTRYPOINT.
../PX4NuttX/nuttx/include/nuttx/config.h:392:#ifndef CONFIG_USER_ENTRYPOINT
../PX4NuttX/nuttx/include/nuttx/config.h:393:# define CONFIG_USER_ENTRYPOINT user_start
../PX4NuttX/nuttx/tools/cfgdefine.c:66: "CONFIG_USER_ENTRYPOINT", /* Name of entry point function */
../PX4NuttX/nuttx/tools/mkconfig.c:266: printf(" * with existing code, for builds which do not define CONFIG_USER_ENTRYPOINT.\n");
../PX4NuttX/nuttx/tools/mkconfig.c:268: printf("#ifndef CONFIG_USER_ENTRYPOINT\n");
../PX4NuttX/nuttx/tools/mkconfig.c:269: printf("# define CONFIG_USER_ENTRYPOINT user_start\n");
../PX4NuttX/nuttx/sched/os_bringup.c:219: * is given by the definitions, CONFIG_USER_ENTRYPOINT. In the kernel
../PX4NuttX/nuttx/sched/os_bringup.c:232: (main_t)CONFIG_USER_ENTRYPOINT,
最后一条信息实际上就告诉了我们 “CONFIG_USER_ENTRYPOINT”是如何调用的。
/****************************************************************************
* Name: os_bringup
*
* Description:
* Start all initial system tasks. This does the "system bring-up" after
* the conclusion of basic OS initialization. These initial system tasks
* may include:
*
* - pg_worker: The page-fault worker thread (only if CONFIG_PAGING is
* defined.
* - work_thread: The work thread. This general thread can be used to
* perform most any kind of queued work. Its primary
* function is to serve as the "bottom half" of device
* drivers.
*
* And the main application entry point:
* symbols:
*
* - USER_ENTRYPOINT: This is the default user application entry point.
*
****************************************************************************/
int os_bringup(void)
{
int taskid;
/* Setup up the initial environment for the idle task. At present, this
* may consist of only the initial PATH variable. The PATH variable is
* (probably) not used by the IDLE task. However, the environment
* containing the PATH variable will be inherited by all of the threads
* created by the IDLE task.
*/
#if !defined(CONFIG_DISABLE_ENVIRON) && defined(CONFIG_PATH_INITIAL)
(void)setenv("PATH", CONFIG_PATH_INITIAL, 1);
#endif
/* Start the page fill worker kernel thread that will resolve page faults.
* This should always be the first thread started because it may have to
* resolve page faults in other threads
*/
#ifdef CONFIG_PAGING
svdbg("Starting paging thread\n");
g_pgworker = KERNEL_THREAD("pgfill", CONFIG_PAGING_DEFPRIO,
CONFIG_PAGING_STACKSIZE,
(main_t)pg_worker, (FAR char * const *)NULL);
DEBUGASSERT(g_pgworker > 0);
#endif
/* Start the worker thread that will serve as the device driver "bottom-
* half" and will perform misc garbage clean-up.
*/
#ifdef CONFIG_SCHED_WORKQUEUE
#ifdef CONFIG_SCHED_HPWORK
#ifdef CONFIG_SCHED_LPWORK
svdbg("Starting high-priority kernel worker thread\n");
#else
svdbg("Starting kernel worker thread\n");
#endif
g_work[HPWORK].pid = KERNEL_THREAD(HPWORKNAME, CONFIG_SCHED_WORKPRIORITY,
CONFIG_SCHED_WORKSTACKSIZE,
(main_t)work_hpthread, (FAR char *const *)NULL);
DEBUGASSERT(g_work[HPWORK].pid > 0);
/* Start a lower priority worker thread for other, non-critical continuation
* tasks
*/
#ifdef CONFIG_SCHED_LPWORK
svdbg("Starting low-priority kernel worker thread\n");
g_work[LPWORK].pid = KERNEL_THREAD(LPWORKNAME, CONFIG_SCHED_LPWORKPRIORITY,
CONFIG_SCHED_LPWORKSTACKSIZE,
(main_t)work_lpthread, (FAR char *const *)NULL);
DEBUGASSERT(g_work[LPWORK].pid > 0);
#endif /* CONFIG_SCHED_LPWORK */
#endif /* CONFIG_SCHED_HPWORK */
#if defined(CONFIG_NUTTX_KERNEL) && defined(CONFIG_SCHED_USRWORK)
/* Start the user-space work queue */
DEBUGASSERT(USERSPACE->work_usrstart != NULL);
taskid = USERSPACE->work_usrstart();
DEBUGASSERT(taskid > 0);
#endif
#endif /* CONFIG_SCHED_WORKQUEUE */
/* Once the operating system has been initialized, the system must be
* started by spawning the user init thread of execution. This is the
* first user-mode thead.
*/
svdbg("Starting init thread\n");
/* Perform any last-minute, board-specific initialization, if so
* configured.
*/
#ifdef CONFIG_BOARD_INITIALIZE
board_initialize();
#endif
/* Start the default application. In a flat build, this is entrypoint
* is given by the definitions, CONFIG_USER_ENTRYPOINT. In the kernel
* build, however, we must get the address of the entrypoint from the
* header at the beginning of the user-space blob.
*/
#ifdef CONFIG_NUTTX_KERNEL
DEBUGASSERT(USERSPACE->us_entrypoint != NULL);
taskid = TASK_CREATE("init", SCHED_PRIORITY_DEFAULT,
CONFIG_USERMAIN_STACKSIZE, USERSPACE->us_entrypoint,
(FAR char * const *)NULL);
#else
taskid = TASK_CREATE("init", SCHED_PRIORITY_DEFAULT,
CONFIG_USERMAIN_STACKSIZE,
(main_t)CONFIG_USER_ENTRYPOINT,
(FAR char * const *)NULL);
#endif
ASSERT(taskid > 0);
/* We an save a few bytes by discarding the IDLE thread's environment. */
#if !defined(CONFIG_DISABLE_ENVIRON) && defined(CONFIG_PATH_INITIAL)
(void)clearenv();
#endif
return OK;
}
在这里我们看到我们的入口函数是作为操作系统的 init进程在运行的。熟悉 Linux系统的人想必都知道 init进程在 Linux系统中的特殊地位,这里我就不介绍了。
在分析脚本的时候,我看到 IO板没有 g_builtins数组于是就认为 IO板是跑裸机的,其实这只是其中一个原因,还有一个原因是 IO板没有使用根文件系统。因为通常跑操作系统都离不开根文件系统,这应该是我的定势思维在作祟了,于是就认为 IO板如果运行 Nuttx系统也必须有一个根文件系统。而我在观察编译输出信息的时候也确实只有核心板才会去制作根文件系统,并且进行链接。后面我看到 IO板的编译使用了 Nuttx库,于是我不得不承认 IO板确实是跑操作系统的。直到此时我心中的疑团才算是解开了,因为我们的 init进程根本就没有使用根文件系统,也没有使用 g_builtins数组,既然不使用,那自然也就不需要。
若是我们想知道 “os_bringup”函数是如何被使用的,这倒也简单。在 “__start”函数的最后调用了一个 “os_start”函数,其源码如下:
/****************************************************************************
* Name: os_start
*
* Description:
* This function is called to initialize the operating system and to spawn
* the user initization thread of execution
*
****************************************************************************/
void os_start(void)
{
int i;
slldbg("Entry\n");
/* Initialize RTOS Data ***************************************************/
/* Initialize all task lists */
dq_init(&g_readytorun);
dq_init(&g_pendingtasks);
dq_init(&g_waitingforsemaphore);
#ifndef CONFIG_DISABLE_SIGNALS
dq_init(&g_waitingforsignal);
#endif
#ifndef CONFIG_DISABLE_MQUEUE
dq_init(&g_waitingformqnotfull);
dq_init(&g_waitingformqnotempty);
#endif
#ifdef CONFIG_PAGING
dq_init(&g_waitingforfill);
#endif
dq_init(&g_inactivetasks);
sq_init(&g_delayed_kufree);
#if defined(CONFIG_NUTTX_KERNEL) && defined(CONFIG_MM_KERNEL_HEAP)
sq_init(&g_delayed_kfree);
#endif
/* Initialize the logic that determine unique process IDs. */
g_lastpid = 0;
for (i = 0; i < CONFIG_MAX_TASKS; i++)
{
g_pidhash[i].tcb = NULL;
g_pidhash[i].pid = INVALID_PROCESS_ID;
}
/* Assign the process ID of ZERO to the idle task */
g_pidhash[ PIDHASH(0)].tcb = &g_idletcb.cmn;
g_pidhash[ PIDHASH(0)].pid = 0;
/* Initialize the IDLE task TCB *******************************************/
/* Initialize a TCB for this thread of execution. NOTE: The default
* value for most components of the g_idletcb are zero. The entire
* structure is set to zero. Then only the (potentially) non-zero
* elements are initialized. NOTE: The idle task is the only task in
* that has pid == 0 and sched_priority == 0.
*/
bzero((void*)&g_idletcb, sizeof(struct task_tcb_s));
g_idletcb.cmn.task_state = TSTATE_TASK_RUNNING;
g_idletcb.cmn.entry.main = (main_t)os_start;
/* Set the IDLE task name */
#if CONFIG_TASK_NAME_SIZE > 0
strncpy(g_idletcb.cmn.name, g_idlename, CONFIG_TASK_NAME_SIZE-1);
#endif /* CONFIG_TASK_NAME_SIZE */
/* Configure the task name in the argument list. The IDLE task does
* not really have an argument list, but this name is still useful
* for things like the NSH PS command.
*
* In the kernel mode build, the arguments are saved on the task's stack
* and there is no support that yet.
*/
#if defined(CONFIG_CUSTOM_STACK) || !defined(CONFIG_NUTTX_KERNEL)
#if CONFIG_TASK_NAME_SIZE > 0
g_idletcb.argv[0] = g_idletcb.cmn.name;
#else
g_idletcb.argv[0] = (char*)g_idlename;
#endif /* CONFIG_TASK_NAME_SIZE */
#endif /* CONFIG_CUSTOM_STACK || !CONFIG_NUTTX_KERNEL */
/* Then add the idle task's TCB to the head of the ready to run list */
dq_addfirst((FAR dq_entry_t*)&g_idletcb, (FAR dq_queue_t*)&g_readytorun);
/* Initialize the processor-specific portion of the TCB */
up_initial_state(&g_idletcb.cmn);
/* Initialize RTOS facilities *********************************************/
/* Initialize the semaphore facility(if in link). This has to be done
* very early because many subsystems depend upon fully functional
* semaphores.
*/
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (sem_initialize != NULL)
#endif
{
sem_initialize();
}
/* Initialize the memory manager */
{
FAR void *heap_start;
size_t heap_size;
/* Get the user-mode heap from the platform specific code and configure
* the user-mode memory allocator.
*/
up_allocate_heap(&heap_start, &heap_size);
kumm_initialize(heap_start, heap_size);
#if defined(CONFIG_NUTTX_KERNEL) && defined(CONFIG_MM_KERNEL_HEAP)
/* Get the kernel-mode heap from the platform specific code and configure
* the kernel-mode memory allocator.
*/
up_allocate_kheap(&heap_start, &heap_size);
kmm_initialize(heap_start, heap_size);
#endif
}
/* Initialize tasking data structures */
#if defined(CONFIG_SCHED_HAVE_PARENT) && defined(CONFIG_SCHED_CHILD_STATUS)
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (task_initialize != NULL)
#endif
{
task_initialize();
}
#endif
/* Initialize the interrupt handling subsystem (if included) */
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (irq_initialize != NULL)
#endif
{
irq_initialize();
}
/* Initialize the watchdog facility (if included in the link) */
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (wd_initialize != NULL)
#endif
{
wd_initialize();
}
/* Initialize the POSIX timer facility (if included in the link) */
#ifndef CONFIG_DISABLE_CLOCK
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (clock_initialize != NULL)
#endif
{
clock_initialize();
}
#endif
#ifndef CONFIG_DISABLE_POSIX_TIMERS
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (timer_initialize != NULL)
#endif
{
timer_initialize();
}
#endif
/* Initialize the signal facility (if in link) */
#ifndef CONFIG_DISABLE_SIGNALS
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (sig_initialize != NULL)
#endif
{
sig_initialize();
}
#endif
/* Initialize the named message queue facility (if in link) */
#ifndef CONFIG_DISABLE_MQUEUE
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (mq_initialize != NULL)
#endif
{
mq_initialize();
}
#endif
/* Initialize the thread-specific data facility (if in link) */
#ifndef CONFIG_DISABLE_PTHREAD
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (pthread_initialize != NULL)
#endif
{
pthread_initialize();
}
#endif
/* Initialize the file system (needed to support device drivers) */
#if CONFIG_NFILE_DESCRIPTORS > 0
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (fs_initialize != NULL)
#endif
{
fs_initialize();
}
#endif
/* Initialize the network system */
#ifdef CONFIG_NET
#if 0
if (net_initialize != NULL)
#endif
{
net_initialize();
}
#endif
/* The processor specific details of running the operating system
* will be handled here. Such things as setting up interrupt
* service routines and starting the clock are some of the things
* that are different for each processor and hardware platform.
*/
up_initialize();
/* Initialize the C libraries (if included in the link). This
* is done last because the libraries may depend on the above.
*/
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
if (lib_initialize != NULL)
#endif
{
lib_initialize();
}
/* IDLE Group Initialization **********************************************/
/* Allocate the IDLE group and suppress child status. */
#ifdef HAVE_TASK_GROUP
DEBUGVERIFY(group_allocate(&g_idletcb));
#endif
/* Create stdout, stderr, stdin on the IDLE task. These will be
* inherited by all of the threads created by the IDLE task.
*/
DEBUGVERIFY(group_setupidlefiles(&g_idletcb));
/* Complete initialization of the IDLE group. Suppress retention
* of child status in the IDLE group.
*/
#ifdef HAVE_TASK_GROUP
DEBUGVERIFY(group_initialize(&g_idletcb));
g_idletcb.cmn.group->tg_flags = GROUP_FLAG_NOCLDWAIT;
#endif
/* Bring Up the System ****************************************************/
/* Create initial tasks and bring-up the system */
DEBUGVERIFY(os_bringup());
/* The IDLE Loop **********************************************************/
/* When control is return to this point, the system is idle. */
sdbg("Beginning Idle Loop\n");
for (;;)
{
/* Perform garbage collection (if it is not being done by the worker
* thread). This cleans-up memory de-allocations that were queued
* because they could not be freed in that execution context (for
* example, if the memory was freed from an interrupt handler).
*/
#ifndef CONFIG_SCHED_WORKQUEUE
/* We must have exclusive access to the memory manager to do this
* BUT the idle task cannot wait on a semaphore. So we only do
* the cleanup now if we can get the semaphore -- this should be
* possible because if the IDLE thread is running, no other task is!
*/
if (kmm_trysemaphore() == 0)
{
sched_garbagecollection();
kmm_givesemaphore();
}
#endif
/* Perform any processor-specific idle state operations */
up_idle();
}
}
我们其实只关心最后一段代码就可以了。在 for循环前面通过语句 “DEBUGVERIFY(os_bringup());”调用了 “os_bringup”函数。
以上就是 IO板启动的大致流程。而我们写程序的时候其实只需简单认为 “user_start”就是我们的入口函数,即当作裸机运行时的 main函数即可。于是我们后面的代码都将以 “user_start”函数作为起点,如果我们忽略操作系统细节的话。
2. firmware
现在从入口函数开始分析我们的 IO板。
在入口函数中,调用的第一个函数是 “up_cxxinitialize”:
/* run C++ ctors before we go any further */
up_cxxinitialize();
注释说这里调用了 C++,
/************************************************************************************
* Private Types
************************************************************************************/
/* This type defines one entry in initialization array */
typedef void (*initializer_t)(void);
/************************************************************************************
* External references
************************************************************************************/
/* _sinit and _einit are symbols exported by the linker script that mark the
* beginning and the end of the C++ initialization section.
*/
extern initializer_t _sinit;
extern initializer_t _einit;
/* _stext and _etext are symbols exported by the linker script that mark the
* beginning and the end of text.
*/
extern uint32_t _stext;
extern uint32_t _etext;
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
/****************************************************************************
* Name: up_cxxinitialize
*
* Description:
* If C++ and C++ static constructors are supported, then this function
* must be provided by board-specific logic in order to perform
* initialization of the static C++ class instances.
*
* This function should then be called in the application-specific
* user_start logic in order to perform the C++ initialization. NOTE
* that no component of the core NuttX RTOS logic is involved; This
* function defintion only provides the 'contract' between application
* specific C++ code and platform-specific toolchain support
*
***************************************************************************/
__EXPORT void up_cxxinitialize(void)
{
initializer_t *initp;
cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n",
&_sinit, &_einit, &_stext, &_etext);
/* Visit each entry in the initialzation table */
for (initp = &_sinit; initp != &_einit; initp++)
{
initializer_t initializer = *initp;
cxxdbg("initp: %p initializer: %p\n", initp, initializer);
/* Make sure that the address is non-NULL and lies in the text region
* defined by the linker script. Some toolchains may put NULL values
* or counts in the initialization table
*/
if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext)
{
cxxdbg("Calling %p\n", initializer);
initializer();
}
}
}
通过源码我们看到初始化是通过段进行调用的。通过段去调用一个函数这在 Linux中国可是家常便饭,像驱动跟板子初始化可都是这么调用的。
最直接的方式自然是去分析这个段,看下到底哪些数据被放进这个段了。不过这可不简单,费时而且费力。
但其实我们可以在这里耍点小聪明。
首先我知道 “_sinit”是该段的起始地址, “_einit”是该段的结束地址。而这两个地址通常都是在链接脚本中定义的,即 ld.script文件。其中就有这样一段内容:
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
代码注释说调用的是 C++,这里注释说该段放的是构造函数。所以我们不可能到 C源文件中国去寻找这些函数。而我们在 map文件中又可以找到下面的内容:
.init_section 0x0800979c 0x4
0x0800979c _sinit = ABSOLUTE (.)
*(.init_array .init_array.*)
.init_array 0x0800979c 0x4 /home/bitcraze/apm/PX4Firmware/Build/px4io-v2_default.build//home/bitcraze/apm/PX4Firmware/src/modules/px4iofirmware/module.pre.o
0x080097a0 _einit = ABSOLUTE (.)
我们可以从这段内容了解以下信息:
一、段地址。起始地址跟结束地址。
二、段大小。已经给出为 0x4,也可以自己计算,这刚好够放一个函数的地址。
三、该段的内容来自那个目标文件。通过这个内容我们其实可以知道具体的目录 “PX4Firmware/src/modules/px4iofirmware”。
既然知道了目录,那么我们就可以到相应的目录下去查看 “module.mk”文件。
SRCS = adc.c \
controls.c \
dsm.c \
px4io.c \
registers.c \
safety.c \
sbus.c \
../systemlib/up_cxxinitialize.c \
../systemlib/perf_counter.c \
mixer.cpp \
../systemlib/mixer/mixer.cpp \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \