2.3 进阶实验

本节导航

2.3.1 选用其他固件版本（或自己的固件）

  • 如果您需要使用自己的PX4固件代码，请将您的代码文件夹改名为Firmware，并压缩为Firmware.zip文件，然后根据2.FirmwareZip\ readme.txt规则重命名，并选择需要固件版本。
  • 例如，自己基于PX4 1.10开发的代码，命名为PX4Firmware1.10.2.zip”，并替换原来掉2.FirmwareZip文件夹下的同名文件，在下图安装选项中的固件版本处选择“4”即可。是否屏蔽PX4输出项目选择“是”，脚本会自动完成所有需要的固件修改以适配本平台


    注：也可以将PX4Firmware1.10.2.zip官方固件的增量文件打包并命名为PX4Firmware1.10.2Update.zip的格式放在2.FirmwareZip目录，安装时会自动拷贝到固件文件夹

2.3.2 ADRC控制器设计与实验

  • 双击打开RflySimAPIs\FlightControlExpCourse\demos\ADRCDemo\ControlMode_FLY.slx即可看到主程序，本程序会自动调用Init_control.m完成参数调用、uORB消息定义等工作
  • 点击编译按钮，上传固件到Pxhawk，然后按照ppts目录的资料进行实验即可。

2.3.3 用Simulink控制器替换PX4官方滤波器、混控器等APP的方法

2.3.3.1 PX4模块替换方法总体介绍

  • 生成的Simulink代码也可以用于替换如上图所示PX4控制软件的某些原生模块（传感器、滤波器、姿态控制器等），但是需要手动修改PX4固件代码来屏蔽原始模块的输出接口。
  • 例如，想用Simulink实现一个滤波器模块（输入传感器数据，输出状态滤波数据）来替换PX4原始的滤波器，需要手动屏蔽图中的“Position & AttitudeEstimator”滤波器模块的输出消息（估计的姿态），然后向uORB消息池发布滤波后姿态数据（对应名为vehicle_attitude的uORB消息）。
  • 一个典型的操作示例为： 1） 打开“Firmware\src\modules\ekf2\ekf2_main.cpp”文件（扩展卡尔曼滤波器模块对应代码）中； 2）在其中屏蔽掉“ORB_ID(vehicle_attitude)”消息相关的发送代码。例如，搜索关键字“_att_pub”的代码行，并找到其中含有“publish” 和“att” 的发送代码行，将其替换为“UNUSED(att);”。这里UNUSED用于防止编译器出现变量未使用的警告。 3）在Simulink中编写姿态滤波器，并用uORB Write模块发送vehicle_attitude消息即可实现姿态滤波器功能的替换。
    • 注：另一种可行方法是修改PX4模块启动脚本文件"Firmware\ROMFS\px4fmu_common\init.d\rcS"，并注释掉想要屏蔽的模块
    • 更详细的教程和例程见“RflySimAPIs\FlightControlExpCourse\demos\ReplacePX4Module”

2.3.3.2 模块替换技术要点

在PX4控制器的基础上，开发顶层算法（或替换某些功能）的技术要点如下：
1). Simulink生成的控制器导入后，可以看出是PX4系统的一个子模块，它可以通过uORB消息系统读取并发送任意消息。
2). Simulink控制器可以通过uORB消息订阅PX4内所有消息，因此可以在PX4现有的滤波器、姿态控制器、轨迹控制器等基础上，进行顶层开发。
3). Simulink控制器模块与PX4内部子模块是相互独立，并行运行的。因此，只需要将PX4模块的输出消息屏蔽掉，用Simulink控制器发送该消息，就能实现模块的替换。

2.3.3.3 RflySim平台配置

设使用的硬件为px4_fmu-v5_default，固件版本为1.12.3，机架类型多旋翼，下面以替换PX4飞控原有的姿态控制器为例，介绍开发流程。

  1. 重新运行安装脚本，根据自身硬件选择合适的编译命令（本例是px4_fmu-v5_default）和固件版本（这里是“6”：最新的PX4 1.12.3版本固件），选择预编译固件“是”，以及是否屏蔽PX4输出选“否”，使用未修改的PX4原始固件代码：

    注意：在弹出的警告框中，选择“PX4官方固件”来解除输出屏蔽
  2. 打开PX4的Firmware文件夹，去模块文件夹（Firmware\src\modules）寻找姿态控制器的相关模块（多旋翼），这里是“mc_rate_control”（角速度控制模块）和“mc_att_control”（角度控制模块）。仔细阅读实现代码，找到关键输入和输出的uORB消息（需要对PX4代码有足够了解，可以通过https://docs.px4.io/master/en/development/development.html来学习，同时需要有一定的飞控理论知识）。

2.3.3.4 代码阅读与接口搜寻

  1. 阅读代码构架。在本例中，我们需要通过Simulink设计一个最简单的姿态控制器，用于替换掉PX4原生的姿态控制器。 一个最基本的姿态控制器通常包含两个环路，角速度环控制器和姿态环控制器（PX4代码中对应mc_rate_control和mc_att_control两个模块），输入信号包括来自滤波器（姿态解算器）的姿态角速度（对应PX4中的vehicle_angular_velocity消息）和姿态角度信号（对应PX4中的vehicle_attitude消息）；如果是手动控制，还需要接收遥控器的输入（对应PX4的manual_control_setpoint），如果是自动控制需要接收上次控制器的期望姿态角（对应PX4中的vehicle_attitude_setpoint）；输出信号是期望控制力矩信号（PX4中对应actuator_controls消息，再经过混控器/控制分配器Mixer可计算得到电机的PWM输出）。下图展示了自动控制框架下，姿态控制器的输入输出信号。

    在PX4 1.11~PX4 1.12版本固件中，姿态控制器由mc_rate_control和mc_att_control两个模块组成，可以在PX4PSP\Firmware\src\modules目录找到这这两个模块，模块内部文件组成如下图所示


    可以看到每个模块都包含一个.cpp文件作为主文件，一个.hpp文件作为头文件，以及CMakeList.txt用于编译调用，_params.c文件用于QGC参数配置。然后，可以打开MulticopterRateControl.cpp文件内的run函数，看一下姿态角速度环控制器的基本构架

    注意1：在PX4 1.10版本之前，姿态控制器只有mc_att_control一个模块，包含了角速度和姿态控制器的功能，到1.11版本才细分为两个模块。
    注意2：固定翼的姿态控制模块为fw_att_control，垂直起降VTOL飞机的姿态控制模块为vtol_att_control。因此，不同的机型需要找到对应的控制器模块来进行构架阅读。

  2. 确定输入信号。结合飞控理论知识，姿态控制器包含了姿态环和角速度环，必需的输入的信号包括姿态角、姿态角速度；如果在手动模式下，还必需获取遥控器信号作为输入。因此，去Firmware文件夹查看PX4代码中姿态控制模块（由mc_rate_control和mc_att_control共同组成）的头文件（即mc_rate_control\MulticopterRateControl.hpp和mc_att_control\mc_att_control.hpp）寻找相关的输入信号的uORB消息定义。
    从上面头文件可知可知，在手动控制模式下，姿态控制器订阅了几条关键uORB消息：角速度vehicle_angular_velocity、角度vehicle_attitude数据、和遥控输入数据manual_control_setpoint。
  3. 确定输入数据结构体。去“C:\PX4PSP\Firmware\msg”路径，可以找到这三条消息
    观看他们的定义如下：

    注意1：如果是自动控制模式，那么姿态控制器的输入应该来自更上层的速度&位置控制器，这个数据为期望姿态角信号，对应的uORB消息为：vehicle_attitude_setpoint。因此，如果需要将本控制器转换为自动控制模式，我们需要将控制输入从manual_control_setpoint变换为vehicle_attitude_setpoint。
    注意2：从代码中可以看到，PX4的姿态控制器订阅的消息非常多，这是因为其中包含了复杂的失效保护逻辑，以确保飞机能够在任意意外情况下实现正确的控制效果。因此，如果要保证替换姿态控制器后，达到足够的安全性和鲁棒性，除了上面介绍的三个消息，还需要订阅更多的飞控状态（如飞机状态vehicle_status、电池状态battery_status等），来编写控制逻辑。

  4. 确定输出信号。结合飞控理论知识和PX4飞控代码，可知关键的输出控制力矩（总拉力f+三个控制力矩τ）消息为actuator_controls_0消息，

    它隶属于“actuator_controls.msg”文件，表示输出归一化的力矩和拉力信息（后面会传送给mixer动力分配模块），本消息msg定义如下：

    八位float32型的control信号的定义（顺序和数据范围）可以参考网页：https://docs.px4.io/master/en/concept/mixing.html#control-group-0-flight-control
  5. 设计替换模块构架。因此，为了能够替换掉飞控的姿态控制器（手动模式），我们需要去代码中屏蔽掉原来PX4姿态控制器mc_rate_control的最关键输出uORB消息，也就是“actuator_controls_0”；然后，再用Simulink编写控制器，订阅角速度vehicle_angular_velocity、角度vehicle_attitude数据、和遥控输入数据manual_control_setpoint，实现姿态控制器，再发送“actuator_controls_0”消息，从而实现姿态控制器的替换。
  6. 屏蔽输出代码。屏蔽“actuator_controls_0”输出消息。 在PX4PSP\Firmware\src\modules\mc_rate_control\MulticopterRateControl.cpp文件中，搜索“actuator_controls_0”，可找到本uORB消息的实例句柄为“_actuators_0_pub”变量

    因此，再搜索“_actuators_0_pub”字符串，找到publish的代码
    _actuators_0_pub.publish(actuators);

    理论上，将其注释掉即可。
    //_actuators_0_pub.publish(actuators);
    但是，由于上述代码中涉及到了actuators变量，直接注释可能导致actuators变量定义了但是未被使用，在PX4严格的代码检查模式，这种不规范的编程行为（定义变量但是未使用，这段代码就没有意义）将会视为错误，导致编译不通过。因此，我们添加UNUSED宏来标注未使用的变量，避免编译器报错。
    //_actuators_0_pub.publish(actuators);
UNUSED(actuators);
  7. 检查修改代码。手动屏蔽代码容易引入一些bug，因此需要再次编译一下，确保代码修改正确。打开桌面Win10WSL快捷方式，输入make px4_fmu-v5_default（如果不是用的Pixhawk 4，请改成你自己的编译命令），如果没有报错，说明代码修改正确

    如果报错了，说明代码修改存在问题，例如，未加分号或括号、未引用变量没加UNUSED等
  8. 测试是否屏蔽成功。将上面生成的固件烧入飞控（在MATLAB中输入“PX4Upload”命令，然后插入飞控即可自动烧入），然后配置好多旋翼HIL仿真模式后，开始HIL仿真并用QGC控制飞机起飞，如果不能起飞，说明输出接口屏蔽成功。或者通过QGC的MAVLink Inspector中观察返回的数据，来看是否有姿态控制数据发出（这种方法适用于其他模块的替换）。
  9. 编写姿态控制器算法。这里我们直接将基础版第04讲的姿态控制器例子（https://rflysim.com/doc/第04讲_实验流程.pdf）进行改造，得到“demo\Exp6_ReplacePX4AttitudeCtrler.slx”如下
  10. 走基础版第04讲的姿态控制器例程中，自动代码生成和烧录流程，确认HIL仿真模式下，飞控能够起飞，并且能够控制姿态（应该不如原生的姿态控制效果好，特别是偏航通道） 注：由于我们这里只修改了遥控器输入下的姿态控制，因此需要用遥控器，将飞控模式切换到自稳模式（CH5或CH6通道），然后用遥控器解锁起飞来测试。
  11. 开发完成后，请务必将修改的代码归回原位，以免影响其他功能的开发。可以重新运行安装脚本，使用如下配置进行固件还原。

    注：PX4 1.11版本的修改方法与PX4 1.12版本完全相同。PX4 1.7到PX4~1.10的修改方法见下一小节。

2.3.3.5 老版本的修改方法

  1. PX4 1.10版本（PX4 1.9版本类似）飞控的修改流程。
    1). 与1.12版本有一定区别，1.10版本只有一个整体姿态控制器模块mc_att_control同时包含角度和角速度环的控制（从PX4 1.11版本才细分为mc_rate_control和mc_att_control两个模块），但是基本功能相同。
    2). 我们需要修改的文件位置是Firmware\src\modules\mc_att_control\mc_att_control_main.cpp
    3). 修改内容有一定区别，搜索“actuator_controls_0”，可找到消息句柄为“_actuators_id”，从而搜索到的publish输出消息为


    由于，输出代码中没有未引用变量的风险，直接注释即可

    4). 其他步骤相同，使用例程文件“RflySimAPIs\FlightControlExpCourse\demos\ReplacePX4Module\Exp6_ReplacePX4AttitudeCtrler.slx”即可。
  2. 1.7版本（1.8版本类似）飞控的修改流程
    1). 与1.10版本有一定区别，1.7版本mc_att_control_main.cpp文件修改的内容为：将“orb_publish(_actuators_id”字符串替换为“//orb_publish(_actuators_id”（即注释掉发送代码）
    2). 在1.7版本中，vehicle_attitude的uORB消息包含了角速度信息（当时还没有vehicle_angular_velocity消息），因此，Simulink控制器的编写与1.10版本有一定区别，只需要订阅vehicle_attitude即可

    3). Simulink控制器使用“RflySimAPIs\FlightControlExpCourse\demos\ReplacePX4Module\Exp6_ReplacePX4AttitudeCtrler107.slx”可完成上述实验。

results matching ""

    No results matching ""