---
# System prepended metadata

title: PX4 模擬 + ulg使用
tags: [操作, PX4, 基本操作]

---

# PX4 模擬
 ## 開啟模擬
 
 #### 如果需要創建文件，在ubuntu中可以用這個指令
 其指令為touch (+你要創建的文件名稱)
``` 
touch （+ 檔案名稱）
```

#### 第一步 : 建議先開啟QGC
必須在你放檔案的地方，才能找到QGC
```
cd Downloads
chmod +x ./QGroundControl-x86_64.AppImage
./QGroundControl-x86_64.AppImage
```
#### 第二步 : 開啟gazebo(需要看自己所下載的版本，此為8)

萬一無法使用
```
pkill -9 px4
pkill -9 -f 'gz sim'
pkill -9 -f gzserver
rm -rf /tmp/px4*
```
一般來說直接用
```
cd PX4-Autopilot
make px4_sitl gz_x500
```
如果要開啟風場，使用以下程式碼開啟
```
PX4_GZ_WORLD=windy make px4_sitl gz_x500
```


#### 第三步 : 開啟連接
```
cd PX4-Autopilot/
MicroXRCEAgent udp4 -p 8888
```
#### 第四步 : 開啟offboard
要先載入基本 ROS2 環境
再進入工作目錄
編譯整個工作空間 (包含所有套件)
載入工作空間環境
執行 offboard_control
```
source /opt/ros/humble/setup.bash
cd ~/ws_sensor_combined/
colcon build
source ~/ws_sensor_combined/install/setup.bash
ros2 run px4_ros_com offboard_control
```

 ## 軌跡(offboard_control.cpp)
  ### 軌跡花花
:::spoiler 軌跡花花
```cpp

/**
 * @brief Offboard control example
 * @file offboard_control.cpp
 * @addtogroup examples
 * @author Mickey Cowden <info@cowden.tech>
 * @author Nuno Marques <nuno.marques@dronesolutions.io>
 */

 #include <px4_msgs/msg/offboard_control_mode.hpp>
 #include <px4_msgs/msg/trajectory_setpoint.hpp>
 #include <px4_msgs/msg/vehicle_command.hpp>
 #include <px4_msgs/msg/vehicle_control_mode.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <stdint.h>
 
 #include <chrono>
 #include <iostream>
 
 using namespace std::chrono;
 using namespace std::chrono_literals;
 using namespace px4_msgs::msg;
 
 class OffboardControl : public rclcpp::Node
 {
 public:
	 OffboardControl() : Node("offboard_control")
	 {
 
		 offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
		 trajectory_setpoint_publisher_ = this->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
		 vehicle_command_publisher_ = this->create_publisher<VehicleCommand>("/fmu/in/vehicle_command", 10);
	 
 
		 offboard_setpoint_counter_ = 0;
 
		 auto timer_callback = [this]() -> void {
 
			 if (offboard_setpoint_counter_ == 10) {
				 // Change to Offboard mode after 10 setpoints
				 this->publish_vehicle_command(VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
 
				 // Arm the vehicle
				 this->arm();
			 }
 
			 // offboard_control_mode needs to be paired with trajectory_setpoint
			 publish_offboard_control_mode();
			 publish_trajectory_setpoint();
		 
 
			 // stop the counter after reaching 11
			 if (offboard_setpoint_counter_ < 11) {
				 offboard_setpoint_counter_++;
			 }
		 };
		 timer_ = this->create_wall_timer(10ms, timer_callback);
	 }
 
	 void arm();
	 void disarm();
 
 private:
	 rclcpp::TimerBase::SharedPtr timer_;
 
	 rclcpp::Publisher<OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
	 rclcpp::Publisher<TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher_;
	 rclcpp::Publisher<VehicleCommand>::SharedPtr vehicle_command_publisher_;
 
	 std::atomic<uint64_t> timestamp_;   //!< common synced timestamped
 
	 uint64_t offboard_setpoint_counter_;   //!< counter for the number of setpoints sent
 
	 double time_counter_ = 0.0;
	 void publish_offboard_control_mode();
	 void publish_trajectory_setpoint();
	 void publish_vehicle_command(uint16_t command, float param1 = 0.0, float param2 = 0.0);
 
 };
 
 /**
  * @brief Send a command to Arm the vehicle
  */
 void OffboardControl::arm()
 {
	 publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
 
	 RCLCPP_INFO(this->get_logger(), "Arm command send");
 }
 
 /**
  * @brief Send a command to Disarm the vehicle
  */
 void OffboardControl::disarm()
 {
	 publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
 
	 RCLCPP_INFO(this->get_logger(), "Disarm command send");
 }
  /**
  * @brief Publish the offboard control mode.
  *        For this trajectory, position, velocity and acceleration controls are active.
  */
 void OffboardControl::publish_offboard_control_mode()
 {
	 OffboardControlMode msg{};
	 msg.position = true;
	 msg.velocity = true;
	 msg.acceleration = true;
	 msg.attitude = false;
	 msg.body_rate = false;
	 msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	 offboard_control_mode_publisher_->publish(msg);
 }
  /**
  * @brief Publish a trajectory setpoint
  *        Implementation of custom trajectory following the provided equations
  */
 void OffboardControl::publish_trajectory_setpoint()
 {
	 // **軌跡參數**
	 const float A = 50.0f;   // 軌跡振幅
	 const float w1 = 0.06f;  // 頻率參數1
	 const float w2 = 0.12f;  // 頻率參數2
	 const float w3 = 0.18f;  // 頻率參數3
 
	 // **時間遞增**
	 time_counter_ += 0.01f;  // 減少變化幅度，讓 PX4 能跟上
 
	 // **計算位置軌跡**
	 float x = 0.5f * A * (std::cos(w1 * time_counter_) + std::cos(w2 * time_counter_));
	 float y = 0.5f * A * (std::sin(w1 * time_counter_) - std::sin(w2 * time_counter_));
	 float z = -20.0f * std::sin(w3 * time_counter_);

	 // **計算速度**
	 float vx = -0.5f * A * (w1 * std::sin(w1 * time_counter_) + w2 * std::sin(w2 * time_counter_));
	 float vy = 0.5f * A * (w1 * std::cos(w1 * time_counter_) - w2 * std::cos(w2 * time_counter_));
	 float vz = -20.0f * w3 * std::cos(w3 * time_counter_);

	 // **計算加速度**
	 float ax = -0.5f * A * (w1 * w1 * std::cos(w1 * time_counter_) + w2 * w2 * std::cos(w2 * time_counter_));
	 float ay = -0.5f * A * (w1 * w1 * std::sin(w1 * time_counter_) - w2 * w2 * std::sin(w2 * time_counter_));
	 float az = 20.0f * w3 * w3 * std::sin(w3 * time_counter_);

	 double yaw = 0.0; // 保持機頭朝前
 
	 TrajectorySetpoint msg{};
	 msg.position = {x, y, z};
	 msg.velocity = {vx, vy, vz};
	 msg.acceleration = {ax, ay, az};
	 
	 msg.yaw = yaw;
	 msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	 
	 trajectory_setpoint_publisher_->publish(msg);
 }
 
 /**
  * @brief Publish vehicle commands
  * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
  * @param param1    Command parameter 1
  * @param param2    Command parameter 2
  */
 void OffboardControl::publish_vehicle_command(uint16_t command, float param1, float param2)
 {
	 VehicleCommand msg{};
	 msg.param1 = param1;
	 msg.param2 = param2;
	 msg.command = command;
	 msg.target_system = 1;
	 msg.target_component = 1;
	 msg.source_system = 1;
	 msg.source_component = 1;
	 msg.from_external = true;
	 msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
	 vehicle_command_publisher_->publish(msg);
 }
 
 int main(int argc, char *argv[])
 {
	 std::cout << "Starting offboard control node..." << std::endl;
	 setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	 rclcpp::init(argc, argv);
	 rclcpp::spin(std::make_shared<OffboardControl>());
 
	 rclcpp::shutdown();
	 return 0;
 }
```
:::





















### 使用Flight Review
[Flight Review](https://review.px4.io/)
### 尋找ulg檔
PX4-AutoPilot -> build -> px4_sitl_default -> rootfs -> log
### 電腦需安裝pyulog
[pyulog](https://github.com/PX4/pyulog)
#### 這之前電腦需安裝python
:::info 萬一無法使用ulog2csv的指令
<font color="#f00">會顯示警告( WARNING):The scripts ulog2csv.exe, ulog2kml.exe, ulog2rosbag.exe, ulog_extract_gps_dump.exe, ulog_info.exe, ulog_messages.exe, ulog_migratedb.exe and ulog_params.exe are installed in '實際位置' which is not on PATH.
  Consider adding this directory to PATH or, if you prefer to suppress this warning, use --no-warn-script-location.Successfully installed pyulog-1.2.0+1.g2ebce87</font>
1. 開啟「開始選單」→ 搜尋 "環境變數"
2. 點擊「編輯系統環境變數」
3. 點「環境變數 (Environment Variables)」
4. 在「使用者變數」裡找到 Path → 點選「編輯」
5. 點「新增」→ 貼上你剛才複製的路徑(實際位置)
6. 按「確定」關閉所有視窗
7. 重新開啟 CMD 視窗（這步很重要）
8. 重新測試
```
ulog2csv --help
```
![螢幕擷取畫面 2025-06-12 074804](https://hackmd.io/_uploads/ByhpS5w7ge.png)
:::




##### 安裝完後，使用以下指令得到csv檔
ulog2csv "檔案位置" -o "csv檔放置位置"
```
ulog2csv "C:\Users\cindy\Downloads\ESO.ulg" -o "C:\Users\cindy\m"
```
### 使用MATLAB得到所需圖形
#### 實際與規劃的三個角度比較圖
:::spoiler MATLAB程式碼
```
% log_plot_attitude_rpy.m
% 比較 attitude (roll, pitch, yaw)，時間從 0 秒開始

% 檔案名稱
filename_ref = '01_vehicle_attitude_setpoint_0.csv';
filename_act = '01_vehicle_attitude_0.csv';

% 讀取資料
data_ref = readtable(filename_ref);
data_act = readtable(filename_act);

% 時間戳轉換並對齊 0
time_ref = data_ref.timestamp * 1e-6;
time_act = data_act.timestamp * 1e-6;
t0 = min([time_ref; time_act]);
time_ref = time_ref - t0;
time_act = time_act - t0;

% 讀取四元數 (規劃)
q0_ref = data_ref.q_0;
q1_ref = data_ref.q_1;
q2_ref = data_ref.q_2;
q3_ref = data_ref.q_3;

% 讀取四元數 (實際)
q0_act = data_act.q0;
q1_act = data_act.q1;
q2_act = data_act.q2;
q3_act = data_act.q3;

% 四元數轉歐拉角
quat_ref = quaternion(q0_ref, q1_ref, q2_ref, q3_ref);
quat_act = quaternion(q0_act, q1_act, q2_act, q3_act);

eul_ref = eulerd(quat_ref, 'ZYX', 'frame');  % Yaw-Pitch-Roll 順序
eul_act = eulerd(quat_act, 'ZYX', 'frame');

% 角度轉換
yaw_ref = eul_ref(:,1);  pitch_ref = eul_ref(:,2);  roll_ref = eul_ref(:,3);
yaw_act = eul_act(:,1);  pitch_act = eul_act(:,2);  roll_act = eul_act(:,3);

% 插值：將 ref 的角度對齊實際時間軸
roll_ref_interp  = interp1(time_ref, roll_ref,  time_act, 'linear', 'extrap');
pitch_ref_interp = interp1(time_ref, pitch_ref, time_act, 'linear', 'extrap');
yaw_ref_interp   = interp1(time_ref, yaw_ref,   time_act, 'linear', 'extrap');

% 有效資料篩選（排除 NaN）
valid = time_act >= 0 & time_act <= 207 & ...
        ~isnan(roll_act) & ~isnan(pitch_act) & ~isnan(yaw_act) & ...
        ~isnan(roll_ref_interp) & ~isnan(pitch_ref_interp) & ~isnan(yaw_ref_interp);

% === 繪圖 ===
figure;
plot(time_act(valid), roll_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_act(valid), roll_act(valid), 'b--', 'LineWidth', 1.5);hold on;
legend('Roll_{ref}', 'Roll'); xlabel('Time (s)'); ylabel('Roll (deg)');
title('Roll Comparison'); grid on; xlim([0 max(time_act(valid))]);

figure;
plot(time_act(valid), pitch_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_act(valid), pitch_act(valid), 'b--', 'LineWidth', 1.5); hold on;
legend('Pitch_{ref}', 'Pitch'); xlabel('Time (s)'); ylabel('Pitch (deg)');
title('Pitch Comparison'); grid on; xlim([0 max(time_act(valid))]);

figure;
plot(time_act(valid), yaw_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_act(valid), yaw_act(valid), 'b--', 'LineWidth', 1.5);hold on;
legend('Yaw_{ref}', 'Yaw'); xlabel('Time (s)'); ylabel('Yaw (deg)');
title('Yaw Comparison'); grid on; xlim([0 max(time_act(valid))]);
```
:::














#### 位置比較圖
:::spoiler MATLAB程式碼
```
% log_plot_interp_xyz.m
% 讀取 PX4 log 的 CSV 檔並對齊時間後繪製本地位置 X、Y、Z

% 檔案名稱
filename_ref = '01_vehicle_local_position_setpoint_0.csv';
filename_pos = '01_vehicle_local_position_0.csv';

% 讀取 CSV 檔案
data_ref = readtable(filename_ref);   % 規劃的 home position
data_pos = readtable(filename_pos);   % 實際飛行軌跡

% 將 timestamp 轉為秒
time_ref = data_ref.timestamp * 1e-6;
time_pos = data_pos.timestamp * 1e-6;

% === 讓時間從 0 秒開始 ===
% t0 = min([time_ref; time_pos]);  % 最早時間
time_ref = time_ref - 100;
time_pos = time_pos - 100;

% 提取位置資料
x_ref = data_ref.x;
y_ref = data_ref.y;
z_ref = data_ref.z;

x = data_pos.x;
y = data_pos.y;
z = data_pos.z;

% 插值參考資料到實際時間軸
x_ref_interp = interp1(time_ref, x_ref, time_pos, 'linear', 'extrap');
y_ref_interp = interp1(time_ref, y_ref, time_pos, 'linear', 'extrap');
z_ref_interp = interp1(time_ref, z_ref, time_pos, 'linear', 'extrap');

% 篩選時間範圍（1～300秒）且去除 NaN 值
valid = time_pos >= 0 & time_pos <= 207 & ...
        ~isnan(x) & ~isnan(y) & ~isnan(z) & ...
        ~isnan(x_ref_interp) & ~isnan(y_ref_interp) & ~isnan(z_ref_interp);

% === 繪圖 ===
figure;
plot(time_pos(valid), x_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_pos(valid), x(valid), 'b--', 'LineWidth', 1.5);hold on;
legend('X_{ref} (Interpolated)', 'X (Actual)');
xlabel('Time (s)'); ylabel('X (m)'); title('X Axis Comparison'); grid on;xlim([0 max(time_pos(valid))]);

figure;
plot(time_pos(valid), y_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_pos(valid), y(valid), 'b--', 'LineWidth', 1.5);hold on;
legend('Y_{ref} (Interpolated)', 'Y (Actual)');
xlabel('Time (s)'); ylabel('Y (m)'); title('Y Axis Comparison'); grid on;xlim([0 max(time_pos(valid))]);

figure;
plot(time_pos(valid), z_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_pos(valid), z(valid), 'b--', 'LineWidth', 1.5);hold on;
legend('Z_{ref} (Interpolated)', 'Z (Actual)');
xlabel('Time (s)'); ylabel('Z (m)'); title('Z Axis Comparison'); grid on;xlim([0 max(time_pos(valid))]);

% === 3D 飛行路徑圖 ===
figure;
plot3(x_ref_interp(valid), y_ref_interp(valid), z_ref_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot3(x(valid), y(valid), z(valid), 'b--', 'LineWidth', 1.5);
legend('Reference Trajectory', 'Actual Trajectory');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('3D Trajectory Comparison');
grid on; axis equal; view(3);
```
:::



















#### 升力比較圖
::: spoiler MATLAB程式碼
```
% log_plot_thrust_compare_fixed.m

% 讀取檔案
file_expect = '01_vehicle_thrust_setpoint_0.csv';
file_actual = '01_vehicle_local_position_setpoint_0.csv';

% 使用 VariableNamingRule = 'preserve' 保留原始名稱
data_expect = readtable(file_expect, 'VariableNamingRule', 'preserve');
data_actual = readtable(file_actual, 'VariableNamingRule', 'preserve');

% % 列出欄位名稱，幫助你找出真正的 thrust Z 軸欄位名（可註解）
% disp('Expect setpoint columns:');
% disp(data_expect.Properties.VariableNames);
% disp('Actual data columns:');
% disp(data_actual.Properties.VariableNames);

% === 這兩行你需要根據上面結果修改欄位名 ===
% 假設欄位名稱為 'xyz_2_' 與 'thrust_2_'（這是 MATLAB 將 xyz[2] 改名後常見的樣子）
thrust_expect = data_expect.('xyz[2]');
thrust_actual = data_actual.('thrust[2]');

% 時間轉秒（從0開始）
time_expect = (data_expect.timestamp - data_expect.timestamp(1)) * 1e-6;
time_actual = (data_actual.timestamp - data_actual.timestamp(1)) * 1e-6;

% 插值對齊
thrust_expect_interp = interp1(time_expect, thrust_expect, time_actual, 'linear', 'extrap');

% 篩選有效資料
valid = time_actual >= 0 & time_actual <= 207 & ...
        ~isnan(thrust_expect_interp) & ~isnan(thrust_actual);

% 繪圖
figure;
plot(time_actual(valid), thrust_expect_interp(valid), 'r', 'LineWidth', 1.5); hold on;
plot(time_actual(valid), thrust_actual(valid), 'b--', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Z-direction Thrust');
title('Expected vs Actual Thrust in Z Direction');
legend('Expected Thrust', 'Actual Thrust');
grid on;
xlim([0 max(time_actual(valid))]);
```
:::



















