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 ""