# 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
```
:::
##### 安裝完後，使用以下指令得到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))]);
```
:::
Sign in with Wallet
Connect another wallet