# Music-Drone-225

**Repository Path**: st0726/music-drone-225

## Basic Information

- **Project Name**: Music-Drone-225
- **Description**: 基于ROS2的PX4四旋翼控制框架
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 2
- **Created**: 2025-07-03
- **Last Updated**: 2025-07-03

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Music-Drone-225

参照[1]中Fast-Drone-250的基于ROS1的PX4控制框架，针对ROS2开发与PX4对接的控制框架，最大的好处是在上位机（机载电脑）可实现对电机油门的直接控制，降低仿真与实际的差距和针对新构型飞机的控制开发难度。

**此控制框架有三种模式：实际飞行、带控仿真、调参**。控制框架已在四旋翼上进行实飞验证，四旋翼由自己搭建（如下图），控制算法参考文献[2]。

<img src="./README.assets/drone.jpg" alt="image-20241118145141215" style="zoom:30%;" />

跟踪轨迹为8字轨迹（最高速度为2m/s），位置曲线如下图所示，跟踪结果良好，目前未出现电机控制延迟而导致的控制失效问题。

<img src="./README.assets/image-20241118145217585.png" alt="image-20241118145217585" style="zoom:50%;" />

**注：**

1. 实际飞行需要增加PX4中EKF2的采样频率，目前可最大增加到400HZ，即上位机发布电机油门频率不会超过400HZ。

2. Linux系统：Ubuntu 20.04

   ROS2版本：Foxy

   PX4版本：1.15.0-1.15.3
   
3. 从ROS2发布电机控制指令到电机实际起转之间的**平均延迟在2.6ms左右，最大为3.8ms**（转速使用自研传感器测量，传感器通信延迟已证实<1ms）

   <img src="./README.assets/ros_step.jpg" alt="ros_step" style="zoom:100%;" />

   

## 前期准备

1. 安装ROS2

   ```bash
   wget http://fishros.com/install -O fishros && . fishros
   ```

2. 升级Cmake

   + 系统自带Cmake所3.16，版本太低不适合后续其他库的安装，这里升级为3.20，参照这里安装https://zhuanlan.zhihu.com/p/519732843

   + 最后需要移动cmake文件

     ```bash
     sudo mv ./cmake-3.20.0 /usr/local/share/cmake-3.20
     ```

2. 安装OSQP求解器

   ```bash
   git clone https://github.com/osqp/osqp
   cd osqp
   mkdir build
   cd build
   cmake -G "Unix Makefiles" ..
   cmake --build .
   cmake --build . --target install
   sudo ldconfig
   ```
   
2. 安装qpOASES求解器

   ```bash
   git clone https://github.com/coin-or/qpOASES.git # 在任意文件夹下就行
   cd qpOASES
   mkdir build
   cd build
   cmake ..
   sudo make
   sudo make install
   ```
   
2. 安装Eigen库

   ```bash
   sudo apt-get install libeigen3-dev
   ```
   
   调整：默认安装路径为`/usr/include/eigen3`，进入到安装目录下`cd /usr/include/eigen3`，执行复制命令，将Eigen文件夹放在/usr/include 下面`sudo cp Eigen/ .. -R`
   
2. 下载并配置1.15.3版本PX4代码（纯仿真跳过此步骤）

   + 下载源码
   
     ```bash
     git clone -b v1.15.3 https://github.com/PX4/PX4-Autopilot.git --recursive
     bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
     cd PX4-Autopilot/
     git submodule update --init --recursive
     git status #查看哪个子模块没下载好，删了那个没下载好的目录重新git submodule
     ```
   
   + 切换版本（针对已下载源码但版本较低的用户）
   
     ```bash
     git describe --tags			# 查看当前版本
     git tag						# 查看可以选择的版本
     git checkout v1.15.3	   	# 比如此处选择 v1.15.3 版本
     git submodule sync --recursive
     git submodule update --init --recursive
     ```
   
   + 修改话题XRCE-DDS发布频率：
   
     在PX4源码`/PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.h.em`文件中，修改
   
     ```c++
     #define UXRCE_DEFAULT_POLL_RATE 1 //数字代表所有话题最大发布周期ms，写1表示按最大频率发布，实际与EKF2的采样频率有关
     ```
   
   + 编译烧录 
   
     ```bash
     make px4_fmu-v6c_default upload	# Pixhawk 6C mini
     make matek_h743-slim upload		# Matek H743-slim V3
     ```
   
   + 在QGC中按照下一节修改参数才能获得最高频率
   
6. 安装Micro XRCE-DDS 代理（纯仿真跳过此步骤）

   需要CMAKE版本3.20以上

   ```bash
   git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
   cd Micro-XRCE-DDS-Agent
   mkdir build
   cd build
   cmake ..
   make
   sudo make install
   sudo ldconfig /usr/local/lib/
   ```

3. 配置PX4飞控和机载电脑的连接（纯仿真跳过此步骤）

   + 这里有设置方式:https://blog.csdn.net/Zecet/article/details/130567392，波特率设置为最高3000000

   + 插入飞控，设置串口权限`sudo chmod 777 /dev/ttyUSB0`

   + 在ubuntu中启动代理`MicroXRCEAgent serial -D /dev/ttyUSB0 -b 3000000`，看是否有话题成功发布标志

## 实际飞行

1. 设置飞机参数

   **基本参数**

   | parameters      | value                   | meaning                                                      |
   | --------------- | ----------------------- | ------------------------------------------------------------ |
   | CBRK_SUPPLY_CHK | 894281                  | 关闭电源检测                                                 |
   | CBRK_USB_CHK    | 197848                  | 关闭USB检查：USB连接下也可以解锁                             |
   | CBRK_IO_SAFETY  | 22027                   | 禁用安全开关：不用安全开关也可解锁                           |
   | PWM_AUX_TIM1    | Dshot600                | 将电调信号改为dshot,响应更快                                 |
   | MPC_THR_HOVER   | 30%                     | 设置悬停油门的大小，(默认值是0.5，请在默认值下，手动飞行后设置该值） |
   | MPC_THR_CURVE   | Rescale to hover thrust | 将中位油门映射到悬停油门 使得手动与定点切换平滑              |
   | THR_MDL_FAC     | 0.6                     | pwm和推力输出的拟合系数（默认值为0.3，5寸桨应该在0.5左右)<br />`rel_thrust = ( THR_MDL_FAC ) * rel_signal^2 + ( 1 - THR_MDL_FAC ) * rel_signal` |

   **EKF2**

   | parameters       | value               | meaning                                          |
   | ---------------- | ------------------- | ------------------------------------------------ |
   | IMU_INTEG_RATE   | 400 Hz              | IMU采样频率，影响ekf2频率                        |
   | IMU_GYRO_RATEMAX | 400 Hz              | 陀螺仪频率，超过IMU_INTEG_RATE会导致ekf2频率下降 |
   | EKF2_HGT_REF     | Barometric pressure | ekf2高度来源：气压计                             |

   **通信**

   | parameters    | value       | meaning                    |
   | ------------- | ----------- | -------------------------- |
   | UXRCE_DDS_CFG | TLELEM1     | 与机载电脑通信串口         |
   | SER_TEL1_BAUD | 3000000 8N1 | 通信波特率，拉到最高       |
   | MAV_0_CONFIG  | TELEM2      | 数传接口                   |
   | SER_TEL2_BAUD | 921600 8N1  | 通信波特率，和数传设置有关 |

   **飞行**

   | parameters       | value | meaning          |
   | ---------------- | ----- | ---------------- |
   | COM_DISARM_PRFLT | -1    | 起飞超时锁定去除 |
   | COM_DISARM_LAND  | -1    | 着陆超时去除     |

   **RC**

   | parameters  | value     | meaning               |
   | ----------- | --------- | --------------------- |
   | RC_MAP_AUX1 | Channel 5 | 摇杆映射（topic输出） |
   | RC_MAP_AUX2 | Channel 6 |                       |
   | RC_MAP_AUX3 | Channel 7 |                       |
   | RC_MAP_AUX4 | Channel 8 |                       |

1. 在QGC中设置遥控器如下

   | 遥控器通道 | topic中的映射 | 功能                        | 在QGC中的设置DOWN | 在QGC中的设置UP/MIDDLE |
   | ---------- | ------------- | --------------------------- | ----------------- | ---------------------- |
   | SA         | AUX1          | 切换手动飞行模式（ON->OFF） | Stablize飞行模式  | 未知                   |
   | SB         | AUX2          | 切换悬停、CMD模式           | 未知              | 未知                   |
   | SC         | AUX3          | 解锁/上锁                   | DISARM            | ARM                    |
   | SD         | AUX4          | 即停开关                    | KILL              | NO KILL                |

2. 启动代理连接PX4

   ```bash
   MicroXRCEAgent serial -D /dev/ttyUSB0 -b 3000000 # 这里根据自己端口号设置
   ```

3. 运行控制器

   ```bash
   git clone https://gitee.com/music-lab/music-drone-225.git
   cd ./Music-Drone-225
   colcon build
   source install/setup.bash
   ros2 launch px4ctrl run_ctrl.launch.py
   ```

## 带控仿真

仿真中遥控器对飞行模式的切换逻辑和实际飞行一致，在rviz中可感受飞行手感来辅助调参。

1. 连接遥控器

   准备RADIOMASTER遥控器，使用USB连接电脑，遥控器弹出模式选择界面选择第一个即可

   编译接口源码

   ```bash
   git clone https://gitee.com/music-lab/joystick_ros2_ws.git
   cd ./joystick_ros2_ws
   colcon build
   source install/setup.bash
   ros2 run joystick_ros2 joystick_ros2
   ```

​		终端出现`Joystick Received!`则表明连接成功

2. 在文件`src/realflight_modules/px4ctrl/include/px4ctrl/input.h`中将`#define SIMULATION`解除注释

3. 运行控制器

   ```bash
   cd ./Music-Drone-225
   ros2 launch px4ctrl run_ctrl.launch.py
   ```

4. 运行Rviz可视化

   ```bash
   ros2 launch odom_visualization run_odom.launch.py
   ```

#### **仿真模型噪声干扰设置**

在`src/uav_simulator/quadsim/include/quadsim/Quadrotor.h`文件中

```C++
#define ADD_NOISE
```

取消注释即可，这里只对角速度的输出增加高斯噪声（`quadsim_node.cpp`中第61行），读者可以自行在其他地方添加高斯噪声。

## 调参

目前可以针对姿态环中串级PID参数进行调整

1. 在仿真模式下，修改文件`src/realflight_modules/px4ctrl/include/px4ctrl/controller.h`中：

   ```c++
   #define DEBUG_ANGLE //取消注释则可指定期望角度，在controller.cpp文件中99行给定期望角度
   #define DEBUG_RATE //取消注释则可指定期望角速度，在controller.cpp文件中115行给定期望角速度
   ```

2. 重新编译运行

## 期望角速度和归一化推力控制（原ROS1控制框架）

像ROS1一样给出的是期望角速度和归一化推力，角速度环是PID，那么需要自定义控制算法，这里已经拟好了框架，只需将文件`src/realflight_modules/px4ctrl/include/px4ctrl/controller.h`中

```C++
#define CTRL_MY      // 自定义控制算法
```

取消注释即可，然后在`controller.cpp`文件中第339和348分别输入自己解算出来的望角速度和归一化推力即可。

第362行是角速度环PID参数和求导，但是对于**角速度导数没有进行低通滤波**，使用者可以自行添加。

## 其他辅助工具

1. ros2bag安装（录制topic话题）

   ```bash
   sudo apt-get install ros-foxy-ros2bag ros-foxy-rosbag2*
   sudo apt install -y ros-foxy-rosbag2-bag-v2-plugins	ros-foxy-rosbag2-converter-default-plugins ros-foxy-rosbag2-storage ros-foxy-rosbag2-transport ros-foxy-rosbag2-storage-default-plugins ros-foxy-ros2bag
   ```

2. plotjuggler安装（数据可视化）

   ```bash
   sudo apt install ros-${ROS_DISTRO}-plotjuggler-ros
   ```

   

## 参考

[1]https://github.com/ZJU-FAST-Lab/Fast-Drone-250

[2] S. Sun, A. Romero, P. Foehn, E. Kaufmann and D. Scaramuzza, "A Comparative Study of Nonlinear MPC and Differential-Flatness-Based Control for Quadrotor Agile Flight," in IEEE Transactions on Robotics, vol. 38, no. 6, pp. 3357-3373, Dec. 2022, doi: 10.1109/TRO.2022.3177279.