如何用开源飞控Pixhawk进行二次开发

Posted 2023-04-20

商业转载请联系作者获得授权，非商业转载请注明出处。
链接：http://www.zhihu.com/question/38874663/answer/84239995
来源：知乎
以下所描述的都是针对px4原生固件，此外，由于固件更新过于频繁，本文描述的是15年7月的固件，主要是举例，有改动的话，自己再研究研究吧（后面换cmake编译方式了，改动蛮大）。

既然要做开发，第一步就是搭好开发环境，根据我的经验，最好是在linux环境下编译，这样效率会很快，以前在windows下编译，经常40分钟以上，这样就太影响开发了；
第二步，大概了解下固件的架构，

如果只涉及应用层的开发，那底层的nuttx系统就可以绕过去了，一般，最好先把uorb模块的机制整明白就好了，从uorb入手，了解每个话题的来源以及作用，整理数据流，清楚每个模块之间的关系即可，比如，要实现手动模式，哪些模块互相交互，auto模式，又有哪些模块起作用，
如果涉及相应算法的开发，要学会定位到相应的算法模块，甚至具体到哪些代码，比如，你想试验你的姿态估计算法，那你就将姿态估计模块替换掉即可，不过相应的接口仍需要和px4环境一样，以姿态估计为例，最后要发布你的vehicle_attitude话题，不然无法与其他模块交互；

另外，不要试图在代码中找main函数，那是单片机思维，你只需看启动脚本即可，\ROMFS\px4fmu_common\init.d\rcs；
第三步，针对你的具体情况，定位相应的模块，进行精读研究，虽然模块基本是用C++写的，但是不会C++也没关系，毕竟又不是让你写，本人倒目前为止，也不会C++，配合注释，看明白就好了，比如，整理下mavlink的控制流程；

px4原生固件模块列表：
系统命令程序
mavlink –通过串口发送和接收mavlink信息
sdlog2 –保存系统日志/飞行数据到SD卡
tests –测试系统中的测试程序
top –列出当前的进程和CPU负载
uORB – 微对象请求代理器-分发其他应用程序之间的信息
驱动
mkblctrl–blctrl电子模块驱动
esc_calib –ESC的校准工具
fmu –FMU引脚输入输出定义
gpio_led –GPIOLED驱动
gps –GPS接收器驱动
pwm –PWM的更新速率命令
sensors –传感器应用
px4io –px4io驱动
uavcan –uavcan驱动
飞行控制的程序
飞行安全和导航
commander –主要飞行安全状态机
navigator –任务，失效保护和RTL导航仪
估计姿态和位置
attitude_estimator_ekf –基于EKF的姿态估计
ekf_att_pos_estimator –基于EKF的姿态和位置估计
position_estimator_inav–惯性导航的位置估计
multirotor姿态和位置控制器
mc_att_control–multirotor姿态控制器
mc_pos_control –multirotor位置控制器
fixedwing姿态和位置控制器
fw_att_control –固定翼飞机的姿态控制
fw_pos_control_l1 –固定翼位置控制器
垂直起降姿态控制器
vtol_att_control –垂直起降姿态控制器
最后提一句，多看看官网的说明，另外根据本人的经验来看，由于大框架，代码人家都写好了，通常你要加功能，所修改的也就几行代码而已，举例说明，比如px4固件只能在手动模式解锁，假如我要修改成定高模式解锁

希望可以帮助你，欢迎采纳 参考技术A Pixhawk一直再用来做项目，没有时间仔细研究代码，因为之前研究过一些无人机开源项目代码，其实大同小异。比如MWC，我之前曾经把里面的代码研一遍，并且进行多次开发，效果也不错。3DR的东西写的比MWC规范多了，应该是更好弄一些，我把之前研究其它开源项目的经验写一下把。
进入一个新的智能硬件开源项目，里面的程序架构是非常关键的，说白了也就是知道文件目录的意义。主文件，控制算法，姿态算法，导航算法，传感器接口，函数定义文件，程序配置文件，特殊变量定义文件等等，这些文件都有标准的名称，很容易区分，而且这些开源程序现在也规矩多了，文件说明也写的很清楚，外国开源项目不像中国某些项目“犹抱琵琶半遮面”，不肯写说明，或者故意写很多垃圾代码。老外放在github上的都很清晰。
弄懂这些就要找准自己二次开发的方向，想多加功能？减少功能？改变算法？然后，注意了，关键：：：：：：：从主函数入手！就算你只是改动一个小地方，也要把主函数搞清楚，然后进入相关函数，或者文件位置进行二次开发。
我还记得自己前两年改MWC改完之后，发现新版竟然跟我改的一样，自己还有些得意，这些年没改代码都有些手痒了。

arduino如何输出一个数组？如位置坐标（x,y）.不是赋值而是输出。

图中（x,y）无法作为两个值输出

参考技术A 可以试一下把println(x,y);改成：
print("("); print(x); print(","); print(y); println(")");
把数组拆分输出 参考技术B

PX4Firmware

经常有人将Pixhawk、PX4、APM还有ArduPilot弄混。这里首先还是简要说明一下：

Pixhawk是飞控硬件平台，PX4和ArduPilot都是开源的可以烧写到Pixhawk飞控中的自驾仪软件，PX4称为原生固件，专为Pixhawk打造。APM(Ardupilot Mega)早期也是一款自驾仪硬件，到APM3.0版本，这款基于Arduino Mega的自驾仪已经走到了它的终点。ArduPilot早期是APM自驾仪的固件，Pixhawk作为APM的升级版，也兼容ArduPilot固件，APM自驾仪卒了之后，ArduPilot现在全面支持Pixhawk，现在大家亲切的称ArduPilot固件为APM。

Pixhawk

APM 2.5

PX4

ArduPilot

笔者一直使用的是Pixhawk飞控，研究PX4Firmware。用Source Insight 看代码是极好的。

PX4固件主要是用C++语言编写，真是学好C++，走遍天下都不怕。使用了NuttX实时操作系统，整体软件架构不可谓不庞大。

不知道如何开头，复述一个人工智能和机器人领域著名的莫拉维克悖论：和传统假设不同，对计算机而言，实现逻辑推理等人类高级智慧只需要相对很少的计算能力，而实现感知、运动等低等级智慧却需要巨大的计算资源。

且从系统说起吧。

地面站配置的文件应该在芯片flash中，格式化SD卡同时擦除芯片后配置信息依然存在。

RTFSC

Pixhawk整体逻辑大致为：

commander和navigator产生期望位置

position_estimator估计当前位置

通过pos_ctrl产生期望姿态

attitude_estimator估计当前姿态

通过att_estimator产生PWM数值

最后通过mixer和motor_driver控制电机

一直都还只是停留在底层，什么时候能感受一下ETHz这帮人的成果呢，这才是pixhawk啊。

启动函数

Pixhawk是没有main函数的，飞控上电后，会自动执行Firmware/ROMFS/px4fmu_common/init.d文件夹下的rcS 启动脚本(startup script)。这个脚本位于被编译到固件中的 ROM文件系统中。这个脚本检测可用的硬件， 加载硬件驱动，并且根据你的设置启动系统正常运行所需的有 app(任务软件 ，包括位置和姿态估计，控制遥测等)。所有属于自启动程序的脚本文件可以在init.d文件夹中找到。

uORB是Pixhawk系统中非常重要且关键的一个模块，它肩负了整数据传输任务，所有的感器、 数据传输任务、 GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。

uORB 的入口点是 uorb_main函数，在这里它检查 uORB的启动参数来完成对应的功能， uORB支持 start/test/status这 3条启动参数，在 PX4的rcS启动脚本中，使用start参数来进行初始化，其他 2个参数分别用来进行uORB功能的自检和列出 uORB的当前状态。

在rcS中使用 start参数启动uORB后，uORB会创建并初始化它的设备实例，其中的实现大部分都在CDev基类完成。

rcS启动顺序

extern “C” __EXPORT int main(int agrc, char *agrv[ ]);

argc和argv是main函数的形参，它们是程序的“命令行参数”。agrc(argument count的缩写，意思是参数个数)，argv(argument vector的缩写，意思是参数向量)，它是一个*char指针数组，数组中每一个元素指向命令行中的一个字符串。

main函数是操作系统调用的，实参只能由操作系统给出。在操作命令状态下，实参是和执行文件的命令一起给出的。例如在DOS、UNIX或Linux等系统的操作命令状态下，在命令行中包括了命令名和需要传给main函数的参数。

命令行的一般形式为：

命令名 参数1 参数2 …… 参数n

命令名和各参数之间用空格分隔。命令名是可执行文件名(此文件包含main函数)。

在rcS执行的时候，比如attitude_estimator_q_main start

那么agrc就等于2，agrv[0]就是attitude_estimator_q_main这个字符串，argv[1]就是start。

所以要判断agrv[1]是start还是stop。

就像你在dos命令行里输入attitude_estimator_q start，自然就给agrc和agrv[]赋值。NuttX系统下的模块的主函数名字都是以”_main”开始的，但是调用的时候不加“_main”。

不管了，就地举个栗子，还是attitude_estimator_q_main.cpp这个文件。

?

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495

extern "C" _EXPORT int attitude_estimator_q_main(int argc, char *argv[]); …… int attitude_estimator_q_main(int argc, char *argv[])  if (argc < 1)  warnx("usage: attitude_estimator_q start|stop|status"); return 1;  if (!strcmp(argv[1], "start"))  if (attitude_estimator_q::instance != nullptr)  warnx("already running"); return 1;  attitude_estimator_q::instance = new AttitudeEstimatorQ; if (attitude_estimator_q::instance == nullptr)  warnx("alloc failed"); return 1;  if (OK != attitude_estimator_q::instance->start())  delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; warnx("start failed"); return 1;  return 0;  if (!strcmp(argv[1], "stop"))  if (attitude_estimator_q::instance == nullptr)  warnx("not running"); return 1;  delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; return 0;  if (!strcmp(argv[1], "status"))  if (attitude_estimator_q::instance)  attitude_estimator_q::instance->print(); warnx("running"); return 0;  else  warnx("not running"); return 1;   warnx("unrecognized command"); return 1; 

在一系列头文件之后，这里extern “C”告诉编译器在编译attitude_estimator_q_main这个函数时按照C的规则去翻译相关的函数名而不是C++的; __EXPORT 表示将函数名输出到链接器(Linker)。

然后跳转到函数的定义部分int attitude_estimator_q_main(int argc, char *argv[])，判断系统给出的命令行的参数，一系列的判断，C++在大型项目上的优势这里有没有发挥出来!总之你要的是start就对了。

和attitude_estimator_q相似，一个正常的应用程序启动如下图所示，直接task_main( ) 吧：

坐标系

惯性导航的基础是精确定义一系列的笛卡儿参考坐标系，每一个坐标系都是正交的右手坐标系或轴系。

对地球上进行的导航，所定义的坐标系要将惯导系统的测量值与地球的主要方向联系起来。也就是说，当在近地面导航时，该坐标系具有实际意义。因此，习惯上将原点位于地球中心、相对于恒星固定的坐标系定义为惯性参考坐标系，下图给出了用于陆地导航的固连于地球的参考坐标系和当地地理导航坐标系以及惯性参考坐标系。

地球坐标系(e系)。原点位于地球中心，坐标轴与地球固连，轴向定义为Oxe,Oye,Oze 。其中，Oze 沿地球极轴方向，Oxe轴沿格林尼泊子午面和地球赤道平面的交线。地球坐标系相对于惯性坐标系绕Ozi 轴以角速度Ω转动。

导航坐标系(n系)。是一种当地地理坐标系，原点位于导航系统所处的位置P点，坐标轴指向北、东和当地垂线方向(向下)。导航坐标系相对于地球固连坐标系的旋转角速率wen取决于P 点相对于地球的运动，通常称为转移速率。

载体坐标系(b系)。一个正交坐标系，轴向分别沿安装有导航系统的运载体的横滚轴、俯仰轴和偏航轴。

在PX4中，

Local position setpoint in NED frame → 导航坐标系(以起飞点home为原点) 北东地 xyz

Global position in WGS84 coordinates → 世界大地坐标系(原点位于地球质心)

NED earth-fixed frame →个人觉得是GPS投影到地面的坐标系,原点?

NED body-fixed frame →机体坐标系，x轴正方向为机头，z轴正方向下

这里有两个函数不得不提：

?

123456789101112131415161718192021222324252627282930313233343536373839404142434445

//将地理学坐标系(geographic coordinate system)中的点(球)投影到本地方位等距平面(XOY)中 int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,float *y)  if (!map_projection_initialized(ref))  return -1;  double lat_rad = lat * M_DEG_TO_RAD; // 度 -> 弧度 A/57.295 double lon_rad = lon * M_DEG_TO_RAD; // GPS数据角度单位为弧度 double sin_lat = sin(lat_rad); //程序中三角运算使用的是弧度 double cos_lat = cos(lat_rad); double cos_d_lon = cos(lon_rad - ref->lon_rad); double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon; if (arg > 1.0)  arg = 1.0;  else if (arg < -1.0)  arg = -1.0; //限幅  double c = acos(arg); double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));// c为正数 *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; return 0; 

将球面坐标转化为平面坐标的过程便称为投影。这里将经纬度转换成地坐标系xy值,也就是说是基于GPS的位置自动控制 。

采用的是等距方位投影的方法(Azimuthal Equidistant Projection)。

方位投影既不是等面积也不是保形的。让φ1和λ0作为投影中心的纬度和经度，则变换方程由下式给出

x=k′cos?sin(λ?λ0)

y=k′[cos?1sin??sin?1cos?sin(λ?λ0)]

这里

k′=csinc

并且

cosc=sin?1sin?+cos?1cos?cos(λ?λ0)]

c在这里代表距中心的角距离(一定点到两物体之间所量度的夹角)。公式的逆表达如下：

?=sin?1(coscsin?1+ysinccos?1c)

以及

λ=???????λ0+tan?1(xsincccos?1cosc?ysin?1sinc),λ0+tan?1(?xy),λ0+tan?1(xy),for?1≠±90°for?1=90°for?1=-90°

到中心的角距离由下式给出：

c=x2+y2??????√

?

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657

//将本地方位等距平面中的点投影到地理学坐标系 int map_projection_global_reproject(float x, float y, double *lat, double *lon)  return map_projection_reproject(&mp_ref, x, y, lat, lon);  __EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)  if (!map_projection_initialized(ref))  return -1;  double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; // 地球半径 double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); double lat_rad; double lon_rad; if (fabs(c) > DBL_EPSILON)  lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));  else  lat_rad = ref->lat_rad; lon_rad = ref->lon_rad;  *lat = lat_rad * 180.0 / M_PI; // 弧度 -> 度 *lon = lon_rad * 180.0 / M_PI; return 0; 

先map_projection_reproject( )再map_projection_project( )。这种方式将位置转换为经纬度和高度， 然后用位置估计参数来更新经纬度和高度，接着转换回位置参考点，属于GPS数据转换的方式。

poll

int poll(struct pollfd fds[], nfds_t nfds, int timeout)

功能：监控文件描述符(多个);

说明：timemout=0,poll()函数立即返回而不阻塞;timeout=INFTIM(-1),poll()会一直阻塞下去，直到检测到return > 0;

参数：

? fds:struct pollfd结构类型的数组;

? nfds:用于标记数组fds中的结构体元素的总数量;

? timeout:是poll函数调用阻塞的时间，单位：毫秒;

返回值：

? >0：数组fds中准备好读、写或出错状态的那些socket描述符的总数量;

? ==0:poll()函数会阻塞timeout所指定的毫秒时间长度之后返回;

? -1:poll函数调用失败;同时会自动设置全局变量errno;

poll( )函数用于监测多个等待事件，若事件未发生，进程睡眠，放弃CPU控制权。若监测的任何一个事件发生，poll函数将唤醒睡眠的进程，并判断是什么等待事件发生，并执行相应的操作。poll( )函数退出后，struct polldf变量的所有值被清零，需要重新设置。

uORB

uORB(Micro Object Request Broker,微对象请求代理器)是PX4/Pixhawk系统中非常重要且关键的一个模块，它肩负了整个系统的数据传输任务，所有的传感器数据、GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。实际上uORB是一套跨「进程」 的IPC通讯模块。在Pixhawk中， 所有的功能被独立以进程模块为单位进行实现并工作。而进程间的数据交互就由为重要，必须要能够符合实时、有序的特点。

Pixhawk使用的是NuttX实时ARM系统，uORB实际上是多个进程打开同一个设备文件，进程间通过此文件节点进行数据交互和共享。进程通过命名的「总线」交换的消息称之为「主题」(topic)，在Pixhawk 中，一个主题仅包含一种消息类型，通俗点就是数据类型。每个进程可以「订阅」或者「发布」主题，可以存在多个发布者，或者一个进程可以订阅多个主题，但是一条总线上始终只有一条消息。

应用层中操作基础飞行的应用之间都是隔离的，这样提供了一种安保模式，以确保基础操作独立的高级别系统状态的稳定性。而沟通它们的就是uORB。

源码中关于uORB有几个常见的函数：

公告主题

在数据被发布到一个主题前，它必须被公告，发布者可以使用下面的API来公告一个新的主题

extern int orb_advertise(const struct orb_metadata *meta, const void *data);

参数：

?meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

data:指向一个已被初始化，发布者要发布的数据存储变量的指针;

返回值：

错误则返回ERROR;成功则返回一个可以发布主题的句柄;如果待发布的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;

公告也可以发布初始化数据到主题，meta参数是传递给API的一个指针，指向由ORB_DEFINE()宏定义好的数据，通常使用ORB_ID()宏来根据主题名称获取该指针。请注意，虽然主题更新可以从中断处理函数发布，公告主题必须在常规的线程上下文中执行。

发布更新

一旦公告了一个主题，公告主题后返回的句柄可使用下面的API来发布主题更新。

extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data);

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

handle:orb_advertise函数返回的句柄;

data:指向待发布数据的指针;

返回值：

OK表示成功;错误返回ERROR;否则则有根据的去设置errno;

uORB不会缓存多个更新，当用户检查一个主题，他们将只能看到最新的更新。

订阅主题

订阅主题的要求如下：

调用ORB_DEFINE()或ORB_DEFINE_OPTIONAL()宏(在订阅者的头文件中包含他们) 发布到主题的数据结构定义(通常与发布者使用同一头文件)

如果满足上面的条件后，订阅者可以使用下面的api来订阅一个主题：

extern int orb_subscribe(const struct orb_metadata *meta);

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

返回值：

错误则返回ERROR;成功则返回一个可以读取数据、更新话题的句柄;如果待订阅的主题没有定义或声明则会返回-1，然后会将errno赋值为ENOENT;

即使订阅的主题没有被公告，但是也能订阅成功;但是在这种情况下，却得不到数据，直到主题被公告;如果可选主题不存在于固件之中，订阅到可选的主题将会失败，但其他主题即便发布者没有进行公告也会订阅成功，这样可大大降低系统对启动顺序的安排。

取消订阅

要取消订阅一个主题，可以用下面的API

extern int orb_unsubscribe(int handle);

拷贝数据

订阅者不能引用ORB中存储的数据或其他订阅共享的数据，而是在订阅者请求时从ORB拷贝数据到订阅者的临时缓冲区。副本拷贝的方式可以避免锁定ORB的问题，并保持两者之间(发布者，订阅者)的API接口简单。它也允许订阅者在必要的时候直接修改拷贝副本的数据供自己使用。从订阅的主题中获取数据并将数据保存到buffer中。

当订阅者想要把主题中的最新数据拷贝一份全新的副本，可以使用：

extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer);

参数：

meta:uORB元对象，可以认为是主题id，一般是通过ORB_ID(主题名)来赋值;

handle:订阅主题返回的句柄;

buffer:从主题中获取的数据;

返回值：

返回OK表示获取数据成功，错误返回ERROR;否则则有根据的去设置errno;

拷贝是以原子操作进行的，所以可以保证获取到发布者最新的数据。

检查更新

订阅者可以使用下面的API来检查一个主题在发布者最后更新后，有没有人调用过orb_copy来接收，处理：

extern int orb_check(int handle, bool *updated);

语句，很多时候Source Insight无法跳转到