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

Posted 2023-04-20

以下所描述的都是针对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改完之后，发现新版竟然跟我改的一样，自己还有些得意，这些年没改代码都有些手痒了。

Pixhawk 添加超声波定高

　　记录一下自己在Pixhawk上添加超声波模块进行定高的过程：

　　硬件：Pixhawk2.4.8（APM3.5固件）、STM32开发板、几块钱一个的超声波模块

　　软件：Mission Planner、Keil（STM32编程用）

 

　　过程：

　　1.分析官方给出的各种RangFinder的添加方案和源码，确定使用STM32单片机读取超声波数据，然后以串口方式发送给Pixhawk飞控。

　　　相关源码链接：https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_RangeFinder

　　　官方给出的传感器添加方式：http://ardupilot.org/copter/docs/common-rangefinder-maxbotixi2c.html

　　　对AP_RangeFinder_LightWareSerial.cpp进行分析：

　　　　

 

　　　通过阅读以上代码，可以得出两点信息：

　　　　a. 数据以字符串形式发送（单位为m），以回车符\'\\r\'结尾；

　　       b.两次数据发送的时间间隔要小于200ms；

 

　　2.在Mission Planner中进行相关配置（以下图片为盗用）：

　　首先配置串口，这里我们配置串口4，波特率为115200，协议选择9:Lidar

　　这里要特别注意，要保证其他的串口协议不为9，否则程序在运行时可能会在其他串口处找数据！

　　

　　其次对RNGFND相关变量进行配置：

 　　RNGFND_TYPE设为8，因为我们参考的是LightWareSerial的数据包格式；

　　RNGFND_MIN_CM和RNGFND_MAX_CM是设置超声波的距离范围；

　　其他几项如下图：

　　

　　配置完之后，点击右侧Write Param；

 

　　3.STM32单片机编程

　　 这里就不再详述，就是配置一个STM32的定时器，输入捕获获取超声波距离值，然后配置一个串口，波特率115200，以字符串形式向外发送数据；

 

　　4.接线，测试：

　　  把两个板子的串口连接起来，打开Mission Planner，就可以在State中的sonarrange处看到我们发过去的距离信息

 　　

　　

 

　　总结：

　　　　以上为串口通信方式添加，如果以IIC或AD方式，只要查看源码对应部分的信息即可，IIC方式是把距离值分成高八位和第八位发出去；

　　　　网上有人用Arduino连接超声波，然后以IIC方式发出去，也是一样的道理；

　　       （以上图片均为盗用，但自己确实实现了，只是不想再去接线）

 

 

　　2018.1.31 补充

　　补充一点这几天调试遇到的问题：

　　1.sonarrange有数据，并不代表飞行时用的就是该数据，要在参数列表中找到EKF2_RANGE_USE_HGT参数，改掉-1，这样才能使用超声波的数据（也有可能是EKF3_RANGE_USE_HGT，需要看自己使能的是哪个扩展卡尔曼滤波）；

　　2.对于pixhawk的定高模式，自己在调试时总是无法实现定高，后来发现问题。这里记录一下自己对如何使用定高模式的理解：

　　　　在定高模式下，油门的40%-60%为死区，也就是说，在这个区间内，飞机会自己调节油门以维持一定高度不变，如果想要改变高度，则把油门拉出该区间即可，此时油门对应的是飞机上升下降的速率，一旦到达想要的高度，立刻把油门拉回该区间，即可实现定高。

　　　　自己的问题在于，在油门为30%左右时由自稳切到定高，切到定高后并未把油门拉到50%左右，以致飞机总是掉高。