飞控学习日志3 | 阿木初级教程笔记

写在前面

本文为学习「阿木实验室」PX4初级课程的一份笔记。

pixhawk底层启动脚本分析

pixhawk底层启动流程分析1

pixhawk底层启动流程分析2

一张很好的启动流程图

教程中有一个pdf「Pixhawk原始固件PX4之常用函数解读」，个人认为该文件应该放在最后看。

pX4系统软件框架

Firmware下有很多文件，其中可能会用到的有Cmake, mavlink, msg, ROMFS 和src这几个文件

src

pixhawk的源码，最重要的文件夹。

src/Drivers

该文件夹含有各种硬件的驱动。

src/Moudles

算法层面的东西都在里面，如姿态控制、位置控制和导航等。

src/examples

学习二次开发很重要的一点就是学习这些例程。

src/Systemcmds

系统指令文件夹，含有飞控支持的命令的源码。

Romfs

文件系统文件夹，里面是飞控的启动脚本，负责系统初始化，类似windows的引导区boot。如果想让自己写的脚本自启动，要在这里面修改。

初始化程序主要在Romfs/Romfs/px4fmu_common/init.d。

cmake

Configs里面是不同硬件的编译脚本，比如nuttx_mindpx-v2_default。如果要添加驱动，得在这个文件里把你新加的驱动加在编译脚本里面，确保它能够被编译到。

mavlink

mavlink是飞控和地面站通信的协议，这个文件夹是一个库，其他文件夹中的源码要调用它。

msg

存放UROB消息主题的地方。

pixhawk的调试

NSH终端是pixhawk常用的数据调试手段

2.插上飞控，打开QGC：

[buglist]:下面两条横线之间的内容是教程中的步骤，但我下下来的QGC直接就是一个AppImage格式的可执行文件，直接对其执行即可，这可能是因为软件更新的原因。
[buglist]:QGC报错： Preflight fail:Accels inconsistent - check out

---

cd  ~/Qt5.7.1/Tools/QtCreator/bins
./qtcreator

在qtcreator下打开项目 >> 找到qgroundcontrol.pro >> Run >> QGC打开

---

3.打开nsh（Nuttx shell）

第五个主图标 >> Mavlink Console，该界面就是命令行调试界面nsh，可以输入命令进行调试。

常见命令：

top：显示飞控当前正在进行的进程。
[buglist]: ctrl + c不能像视频上那样中断输出
help：可以看到系统的根目录和进程列表。我们可以打开任意一个进程，如：

commander start //打开进程
commander status //查看进程状态

Linux技巧：快捷的补全方式：

1. ls >> 双击目标文件 >> 单击鼠标中键。
2. 输入文件名开头，然后按tab键自动补全。解决无法自动补全，vim多行注释及取消，vim保存文件。

相应的源码理解

打开Firmware/src/moudles/commander/commander.cpp，截取代码的一部分，可以看到命令start的源代码。输入回车执行的函数就是commander_main()。

\\argc表示的是命令的个数，共输入argc个参数，argv是一个argc维的字符数组。

int commander_main(int argc, char *argv[]) 
{
	if (argc < 2) {
		usage("missing command");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			PX4_INFO("already running");
			/* this is not an error */
			return 0;
		}

		thread_should_exit = false;

		Commander::main(argc, argv);

		unsigned constexpr max_wait_us = 1000000;
		unsigned constexpr max_wait_steps = 2000;

		unsigned i;
		for (i = 0; i < max_wait_steps; i++) {
			usleep(max_wait_us / max_wait_steps);
			if (thread_running) {
				break;
			}
		}

		return !(i < max_wait_steps);
	}

修改一个脚本并上机运行

官网上的「writing my first application」是一个很好的参考资料。

1.在examples或moudles下新建一个文件夹，里面应该包含.c文件和CMakeLists.txt编译脚本。.c文件和编译脚本都首先应该包含版权声明，然后才是你的代码。
下面是px4_add_moudle的官方说明

#	px4_add_module
#
#	This function builds a static library from a module description.
#
#	Usage:
#		px4_add_module(MODULE <string>
#			[ MAIN <string> ]
#			[ STACK <string> ] !!!!!DEPRECATED, USE STACK_MAIN INSTEAD!!!!!!!!!
#			[ STACK_MAIN <string> ]
#			[ STACK_MAX <string> ]
#			[ COMPILE_FLAGS <list> ]
#			[ INCLUDES <list> ]
#			[ DEPENDS <string> ]
#			[ SRCS <list> ]
#			[ MODULE_CONFIG <list> ]
#			[ EXTERNAL ]
#			)
#
#	Input:
#		MODULE			: unique name of module
#		MAIN			: entry point, if not given, assumed to be library
#		STACK			: deprecated use stack main instead
#		STACK_MAIN		: size of stack for main function
#		STACK_MAX		: maximum stack size of any frame
#		COMPILE_FLAGS		: compile flags
#		LINK_FLAGS		: link flags
#		SRCS			: source files
#		MODULE_CONFIG		: yaml config file(s)
#		INCLUDES		: include directories
#		DEPENDS			: targets which this module depends on
#		EXTERNAL		: flag to indicate that this module is out-of-tree
#		UNITY_BUILD		: merge all source files and build this module as a single compilation unit
#
#	Output:
#		Static library with name matching MODULE.
#
#	Example:
#		px4_add_module(MODULE test
#			SRCS
#				file.cpp
#			STACK_MAIN 1024
#			DEPENDS
#				git_nuttx
#			)

2.修改cmake文件：打开Firmware/cmake/configs，找到自己的硬件所对应的驱动，在里面添加你新建的文件夹的路径并保存，这样系统就将对该文件进行编译。比如添加examples/px4_simple_app，实际上这个行已经有了，只要把注释去掉就行。

3.编译并下载固件：打开Firmware，连上飞控，输入make px4fmu-v3_default进行编译。然后输入make px4fmu-v3_default upload进行下载，完成后需要重新插拔飞控，让bootloader识别到你的烧写动作。如果报错可以多插拔几次。
[buglist]：这次没有报错，我猜想可能是之前下载了错误的版本进去。

4.打开地面站nsh，输入help，可以看到px4_simple_app已经烧写进去了。输入px4simple_app start即可启动该app。观察源码可以看到，px4_simple_app本身并没有任何的if语句去找他后面的命令，因此直接输入px4_simple_app， px4_simple_app lalala，px4_simple_app la la la，效果都是一样的，后面的命令实际都没有被使用。

app输出了一句hello sky和加速度计的信息。要输出加速度计的信息，需要了解uROB。

uROB(Micro Object Request Broker,微对象请求代理器)

oURB是一种高级的多进程之间的通信方式

前置知识：多线程、多进程

要深入理解uROB，必须具备linux下多线程多进程的基础，理解多线程、多进程通信机制消息，管道，共享内存，队列，互斥锁。

整个飞控基于多进程编程，这样写的好处是各个模块分离，一个进程的崩溃不会影响其他进程。

多进程知识笔记

Linux下的多进程编程
[buglist]:这里面的有些例程有点错误，我改过来放到这个博客里了。

进程的概念

进程是正在运行的程序的实例。

进程的结构

Linux下的一个进程在内存中存有3段数据，包括代码段、堆栈段和数据段。代码段存放的是进程的代码；堆栈段存放的是局部变量、子程序的返回地址和子程序的参数；数据段存放的是全局变量、常数以及动态数据所分配的空间。（如malloc函数分配得到的空间）

进程的控制

用函数fork()可以拷贝当前进程创建一个新的进程，用exec函数族可以启动另外的进程来取代当前运行的进程。
1.fork
fork()是一个神奇的函数，调用它一次，他会返回两次值，这是因为在fork()函数中，进程变成了两个，一个是子进程，一个是父进程，两个进程分别执行了return。因此fork()之后，后面的语句也会输出两次，一次是父进程输出的，一次是子进程输出的。
fork() 例程：

#include <unistd.h>  
#include <stdio.h>  
int main(void)  
{  
   int i=0;  
   printf("i son/pa ppid pid  fpid\n");  
   //get_ppid返回当前进程的父进程pid  
   //get_pid返回当前进程的pid,  
   //fpid指fork返回给当前进程的值，若当前进程为
   for(i=0;i<3;i++){  
       pid_t fpid=fork();  
       if(fpid==0)  
           printf("%d child  %4d %4d %4d\n",i,getppid(),getpid(),fpid);  
       else  
           printf("%d parent %4d %4d %4d\n",i,getppid(),getpid(),fpid);  
   }  
   return 0;  
}  

输出：

2.exec函数族
Linux中，exec函数族主要有execl，execlp，execle，execv，execve和execvp。一个进程一旦调用exec函数族，进程的代码段会被替换成新的，堆栈段和数据段也将由系统重新分配，只有进程号仍然不变。程序虽然变了，但对电脑来说，这还是同一个进程。

例程：用fork和execlp从一个进程中打开一个新的进程，但原进程仍在运行。

#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/wait.h>

char command[256]; 
void main() 
{ 
int rtn; /*子进程的返回数值*/ 
while(1) { 
/* 从终端读取要执行的命令 */ 
printf( ">" ); 
fgets( command, 256, stdin ); 
command[strlen(command)-1] = 0; 
if ( fork() == 0 ) { 
/* 子进程执行此命令 */ 
execlp( command, command); 
/* 如果exec函数返回，表明没有正常执行命令，打印错误信息*/ 
perror( command ); 
exit(errno); 
} 
else { 
/* 父进程， 等待子进程结束，并打印子进程的返回值 */ 
wait ( &rtn ); 
printf( " child process return %d/n",rtn ); 
} 
} 
} 

3.system和popen
system函数首先调用fork，然后再调用exec来执行用户的shell，查找可执行文件的命令并分析参数，最后使用wait()函数族之一来等待子进程的结束，子进程结束后即返回原进程。popen函数通过创建一个管道，调用 fork 产生一个子进程，执行一个 shell 以运行命令来开启一个进程。来进行标准的输入和输出。

Linux下的进程间通信

Linux下常见的进程间通信（IPC：InterProcess Communication）有管道、消息队列、共享内存、信号量、套接口和互斥锁等。
1.管道
管道包括有名管道和无名管道两种，前者用于父子进程的通信，后者用于同一台机器上任意两个进程之间的通信。
有名管道：
pipe( int pipefd [2] )，其中piepefd[0]用于读操作，pipefd[1]用于写操作。
有名管道：
在当前目录下创建名为mypipe的有名管道（mknod mypipe p），创建后就可以使用一般的文件I/O函数如open、close、read、write等来对它进行操作。

2.消息队列（Message queue）
消息队列正在逐渐被淘汰。

3.共享内存
共享内存非常快，它由一个进程创建一块共享进程区，其余进程对这块内存区进行读写。想得到一个共享内存区有两种方式，映射设备的内存映像文件和用shm函数族，比较常用的方式是后者。

Linux内核中的每个IPC结构都有一个对应的标识符（非负整数）。

首先用shmget创建一块共享内存。共享内存创建后，其余的进程可以用shmmat将其连接到自身的地址空间内。这个进程就可以对这块地址进行读写了。

int shmget(key_t key, int size, int flag)
该函数按照请求分配的size大小作为共享内存，并返回这个共享内存的标识符，参数key用来变换成一个标识符，而且每一个IPC对象与一个key相对应。

void *shmat(int shmid, void *addr, int flag);
shmid 是共享内存标识符，shmaddr指定共享内存出现在进程内存地址的什么位置，直接指定为NULL让内核自己决定一个合适的地址位置。shmflg为打开模式，SHM_RDONLY：为只读模式，其他为读写模式。该函数的返回值即为该进程数据段所连接的具体地址。

要注意的一点是必须确保当一个进程来读取这个数据的时候，这个数据已经写好了，要做到这一点，就要用到信号量。

4.信号量（Semaphore）
信号量又称信号标，主要应用在共享内存中。一个进程想要获取一块共享内存需要以下几步：1. 测试控制该资源的信号量。2.1 若为正，允许使用该资源，进程将信号量减1。2.2 若信号量为0，说明资源目前不可用，进程休眠，直到信号量为正为止，进程唤醒转入步骤1。4.当进程用完一个信号量控制的资源之后，信号量+1，此时若有进程在等待该资源将被唤醒。
[TODO]: 具体代码学习

5.互斥锁
互斥锁（英语：英语：Mutual exclusion，缩写 Mutex）是一种用于多线程编程中，防止两条线程同时对同一公共资源（比如全局变量）进行读写的机制。
这是一份很好的互斥锁学习中文教程将互斥锁的概念讲的非常清楚。
[TODO]: 具体代码学习

6.套接口
套接口是实现Linux和其他操作系统的主要通信方式之一，WWW、FTP、TELNET服务都是基于套接口编程来实现的。除了不同计算机之间的通信之外，套接口同样适用于同一台计算机内进程的通信。

这个东西飞控好像用不上，先放着吧。

多线程知识学习

Linux下的多线程编程

uROB原理深入理解

uORB深入理解
uROB有点像报纸，发行商发行报纸(pubilish)，用户订阅报纸(subscribe)，然后读报纸获取信息(copy)

常用的uROB接口函数：

poll(): 用在监控某个消息的文件描述符有没有更新。

orb_subscribe(): 想查看某份消息的内容必须先订阅他。

orb_copy(): 相当于将报纸的内容拷贝到你的脑海里。

orb_advertise(): 公告，告诉所有进程，我发布了某某某主题，你们想要的可以来订阅

orb_publish(): 去发布一个新的消息到主题。

orb_unsubscribe(): 取消订阅。

orb_check(): 检查有没有发布新的消息。该函数和poll类似但不会阻塞等待。

orb_set_interval(): 设置订阅的最小时间间隔。

orb_advertise_multi(): 可以发布多个公告。

orb_subscribe_multi(): 订阅多个消息

orb_exist(): 检查某个主题是否存在

uROB例程分析

px4_simple_app是官网自带的例程，它输出一个 “hello,sky” 并输出了加速度计的信息。

知识点：1.消息的阻塞订阅：px4_poll，orb_subscribe,orb_copy。2.终端打印调试语句PX4_INFO。3.消息发布的声明：orb_advertise,orb_publish。

//订阅一个消息ID
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined))
//打开msg，可以看到文件sendor_combined，可以看到有很多不同类型的数据（陀螺仪、加速度传感器，磁力计和气压计等），订阅这些数据即可看到文件中的这些数据。
//设置sensor_sub_fd每个二百毫秒订阅一次消息。

orb_set_interval(sensor_sub_fd, 200)

阻塞等待成功–返回值大于零，阻塞等待超时–返回值小于零

//将数据拷贝出来
struct sensor_combined_s raw;//将数据放到缓冲区，sensor_combined_s是系统自动产生的，
if(阻塞等待成功){
  orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw)
}

将px4_simple_app写入总的编译脚本里：

打开cmake/configs，找到对应硬件的编译脚本，examples/px4_simple_app。

重新编译，make px4fmu-v2_default，编译成功后再上传到硬件make px4_fmu-v2_default upload并重新插拔USB。打开地面站再nsh中输入help可以看到px4_simple_app已经在系统中了，输入px4_simple_app start，就可以看到程序正确地打印了加速度计的数据。

注：px4_simple_app中定义的缓冲区的那个数据结构，系统之所以会自动生成，是因为我们在Msg里面定义了，在编译之后就会在对应的Build文件里显示出来自动生成的数据结构。

uROB自定义主题实例

在moudles下新建两个文件夹px4_uorb_subs和px4_uorb_adver，下面分别写好CMakeLists.txt和.c文件，注意，两个.c文件里都要include uROB/topics/下对应的主题>>在Firmware/cmake/configs的对应固件文件下加入两个Moudles的路径 >> 在msg里加入msg文件，并定义相应的数据结构 >> 在Firmware/msg下的CMakeLists中加入新的msg的名字。 >> 编译make px4fmu-v3_default，编译后对应的topics就会出现在相应的build文件夹里 >> 上传 >> 打开nsh，输入help，如果有你加入的modules说明成功了。
[buglist]:编译报错说没有时间戳，我觉得这可能是版本更新的后，要求msg必须要有时间戳变量了（时间戳记录了消息发布的时间）
注1：文件夹和.c文件名最好对应起来。
注2：在相应build文件中的src/modules/uORB/topics中可以看到编译好的test_urob_s文件。只要在.c文件中include <uORB/topics/test_uorb.h>即可，消息id也是从弄这里引用的。

上电自启动

在ROMFS/px4fmu_common/init.d里打开自启动脚本rc.mc_default（这个是多旋翼的自启动脚本，如果选的airframe是多旋翼，就会执行这个） >> 在文件下加入uorb_adver start>>重新上传，应用就将上电自启动。

Linux技巧: 白色代表是个文件，蓝色代表文件夹，绿色代表可执行文件。

Mavlink协议与飞控实例操纵

对数据流的直观认识

1.裸数据可以用XCOM V2.0看到。

2.用Mission Planner小软件可以看到经过解析的数据流，用vs打开 >> 不调试运行 >>选择对应端口，115200 >> 看到数据流。

3.在nsh中输入mavlink status，可以看到各个数传接口，从接口的type和transpot protocol可以看出各个接口的

注：软件存放地址为：MissionPlanner-masterwjj\MissionPlanner-masterwjj\MissionPlanner-master\ExtLibs\SimpleExample

要读取PM2.5的传感器数据，融合到mavlink的数据流中，并正确地在地面站解析出来。

mavlink协议分析

pixhawk使用mavlink协议实现飞控的数据链传输。mavlink为飞行器和地面站通讯时常常用到的数据制定了发送和接受的规则并加入了校验功能。

mavlink的传输基本单位是消息帧，一帧8-256bytes不等。
STX帧头：0xFE
LEN有效载荷长度：PAYLOAD的数据长度
SEQ消息帧信号：每次增加1，循环使用，用来计算丢包率。丢包率达到百分之三十就已经说明你的信号不太稳定了。
SYS消息系统编号：不同硬件系统编号不同。
COMP：不太重要
MSG消息ID：标志payload里面的消息类型，比如心跳包是零号。
PAYLOAD：有效载荷，0-255bytes
CKA, CKB：最后两位–两个数据校验位，校验通过才认为该数据是正确的。

从一个FE到下一个FE之间就是一包数据。
HEARTBEAT零号包是心跳包，每隔一秒发送一个，如果没接收到，就认为飞控和地面站失联了。共有二百五十五种包。

arm/disarm：解锁/上锁

simple_example小程序分析：程序包含三部分 1.地面站不停地向飞控请求数据 2.地面站解析了这些数据。 3.地面站可以向mavlink发送指令

此处应有各种包的说明网址，px4官网mavlink介绍，QGC官网介绍

mavlink源码分析

源码在modules/mavlink中，主函数在mavlink_main.cpp中。
starthelper

configure_stream()：控制流的发送频率

MavlinkStream：核心的发送消息流的地方。用了一个链表包含了各个消息类型的类。各种类型的包都继承了MavlinkStream。

这些类的写法：
get_name：得到这个包的名字。
get_id_static：得到包的id。
最重要的函数：send，完成了最终的发送工作，将这个类型的数据类型发送了出去，***_send_struct由一个小工具来生成。在结尾有一个类的链表，保存了各个实例化过的mavlink的类。

自定义mavlink函数

__stream链表会被频繁地追加。
stream_configure函数主要有两个用处：1.配置流的发送时间，2.把这个类添加到循环链表里面去。

精简mavlink的数据

1.不去配置他的congire_stream。（在case MAVLINK_MODE_NORMAL处）
2.把链表的实例化注释掉。

自定义mavlink的数据

比如要采集一些无人机上的传感器数据，但这个传感器数据以前没有。

1.自定义一个要发送的类 >> 2.把这个类添加到链表StreamListItem里面 >> 3.在MAVLINK_MODE_NORMAL处configure_stream一下，并配置他的发布时间。>> 编译

类的定义过程有一个很重要的函数：send函数，他是最终的发送函数。他是mavlink库里面带的，不需要自己编写，生成这个函数有一个小工具，根据成员函数生成对应的头文件。

每一个send函数最下面的都有一个**_send_struct函数，这些函数在Firmware下的mavlink/include/mavlink/v1.0/common下可以找到这些头文件，头文件中带的东西和官网各个包的数据类型相同，还包含了编码和解码。

自动生成send函数

[TODO]: 安装mavlink_generater
mavlink_generater使用：
XML：my_test.xml(mavlink/message_definitions)
自定义的话主要替换的就是id(不能与现有的系统id相重合，也不能大于255)和name及成员()
out：生成的路径，新建一个文件夹
语言：C
版本：1.0
回到原来的目录就可以看到库函数已经生成了

用mavlink_generater生成.h文件>> 将生成的文件mavlink_msg_test_type.h复制到自己的源码里面去（Firmware/mavlink/inlude/mavlink/v2.0/commo
n）>>在同目录的common里面把文件路径包含进去 >> 在modules/mavlink里面修改mavlink_message.cpp >> 用Qt打开它，新建一个类MavlinkStreamMytest。 >> 在链表里面实例化一下加到后面的链表里面。 >>在2100行configure_stream()一下设置发送频率 >> 对它进行编译

打开modules里的mavlink的mavlink_main.cpp，找到大循环while(!)。

注1：新建的类的写法，继承MavlinkStream，将get_name, get-name_static, get_size等都重写一遍，最终是send函数。
注2：数据类型__mavlink_
[TODO]:在MAVLINK Common Message查看哪个id没有被用过

PID调参

与IMU替换无关，待完成。

IMU替换

赫星出的pixhawkv2.1没有SPI接口，因此我们选取uart口来装新的传感器。

思路：
1.将sbg system的数据解码出来。
2.在官网上找到uORB消息关系图，将发布vehicle_attitude的语句屏蔽。再自己写一个module，从里面订阅vehicle_attitude，将其复制到缓存区，再将缓存区中的四元数改为从sbg system中读出来的数据，最后将缓存区中的数据公告并发布到主题。

可以看到发布vehicle_attitude的module有ekf2和attitude_estimator_q，查看自启动项，发现只有ekf2使用，看官网的升级日志可以发现，attitude_estimator_q很快将被弃用，因此把ekf2中的发布注释掉即可。如果要发布其他的主题，思路是类似的。

注1：要将原传感器所有发布该主题的语句都屏蔽。
注2：C++中using的用法
注3：NED坐标系：north east down
注4：一个数据的意义。
#define POLLIN 0x001 /* There is data to read. */