PX4实战之通过OFFBOARD模式控制无人机自主飞行

固件版本：PX4 1.14

一、准备工作

1. 添加自定义模块

首先，在 PX4-Autopilot/src/modules 目录下创建一个名为 control_node 的文件夹。

接着，在 control_node 文件夹中创建以下四个文件：

2. 新建并编辑文件

创建 CMakeLists.txt

在 control_node 文件夹中创建 CMakeLists.txt 文件，并将以下内容复制进去：

px4_add_module(
    MODULE modules__control_node
    MAIN control_node
    SRCS
        control_node.cpp
)

创建 Kconfig

创建 Kconfig 文件，并将以下内容复制进去：

menuconfig MODULES_CONTROL_NODE
    bool "control_node"
    default n
    ---help---
        Enable support for control_node

创建 control_node.cpp

创建 control_node.cpp 文件，并将以下内容复制进去：

#include "control_node.h"

#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>

#include <uORB/topics/parameter_update.h>

int ControlNode::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int ControlNode::custom_command(int argc, char *argv[])
{
    /*
    if (!is_running()) {
        print_usage("not running");
        return 1;
    }

    // additional custom commands can be handled like this:
    if (!strcmp(argv[0], "do-something")) {
        get_instance()->do_something();
        return 0;
    }
     */

    return print_usage("unknown command");
}

int ControlNode::task_spawn(int argc, char *argv[])
{
    _task_id = px4_task_spawn_cmd("module",
                                  SCHED_DEFAULT,
                                  SCHED_PRIORITY_DEFAULT,
                                  1024,
                                  (px4_main_t)&run_trampoline,
                                  (char *const *)argv);

    if (_task_id < 0)
    {
        _task_id = -1;
        return -errno;
    }

    return 0;
}

ControlNode *ControlNode::instantiate(int argc, char *argv[])
{
    int example_param = 0;
    bool example_flag = false;
    bool error_flag = false;

    int myoptind = 1;
    int ch;
    const char *myoptarg = nullptr;

    // parse CLI arguments
    while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
    {
        switch (ch)
        {
        case 'p':
            example_param = (int)strtol(myoptarg, nullptr, 10);
            break;

        case 'f':
            example_flag = true;
            break;

        case '?':
            error_flag = true;
            break;

        default:
            PX4_WARN("unrecognized flag");
            error_flag = true;
            break;
        }
    }

    if (error_flag)
    {
        return nullptr;
    }

    ControlNode *instance = new ControlNode(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

ControlNode::ControlNode(int example_param, bool example_flag)
    : ModuleParams(nullptr),_position_smoothing()
{

}


// 定义initialize_smoothing并使用作用域
void ControlNode::initialize_smoothing() {
    // 设置运动约束
    _position_smoothing.setMaxVelocityXY(5.0f);    // 最大水平速度
    _position_smoothing.setMaxVelocityZ(3.0f);     // 最大垂直速度
    _position_smoothing.setMaxAccelerationXY(2.0f); // 最大水平加速度
    _position_smoothing.setMaxAccelerationZ(3.0f);  // 最大垂直加速度
    _position_smoothing.setMaxJerkXY(4.0f);        // 最大水平加加速度
    _position_smoothing.setMaxJerkZ(4.0f);         // 最大垂直加加速度

    // 设置控制参数
    _position_smoothing.setMaxAllowedHorizontalError(2.0f);
    _position_smoothing.setVerticalAcceptanceRadius(0.5f);
    _position_smoothing.setCruiseSpeed(3.0f);
    _position_smoothing.setHorizontalTrajectoryGain(1.0f);
    _position_smoothing.setTargetAcceptanceRadius(0.5f);
}


void ControlNode::run()
{
    // 记录起始点位置
    float begin_x;
    float begin_y;
    float begin_z;

    float_t xy_rad = 1;
    float_t z_rad = 1;
    // float_t yaw_rad = 0.1;
  // 进入offboard模式并解锁
    while (!should_exit())  // 当should_exit()返回false时，循环继续，直到程序退出
    {
        _vehicle_status_sub.copy(&_status);  // 订阅并复制当前的飞行器状态到_status变量中

        // 如果飞行器处于Offboard模式且已经解锁（arming），则跳出循环
        if ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) && (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED))
        {
            PX4_INFO("in offboard and armed");  // 打印信息，表示飞行器已经进入Offboard模式并解锁
            break;  // 跳出循环，结束解锁和模式设置过程
        }

        // 设置命令的目标系统和目标组件
        _command.target_system = _status.system_id;  // 设置目标系统为当前系统ID
        _command.target_component = _status.component_id;  // 设置目标组件为当前组件ID

        ocm.timestamp = hrt_absolute_time();  // 获取当前时间戳，并设置给ocm（Offboard控制模式结构体）

        // 获取当前飞行器的本地位置并复制到_vehicle_local_position
        _vehicle_local_position_sub.copy(&_vehicle_local_position);

        // 设置目标位置（x, y）为当前飞行器的位置，z值则下降5米
        sp_local.x = _vehicle_local_position.x;
        sp_local.y = _vehicle_local_position.y;
        sp_local.z = _vehicle_local_position.z - 5;  // 设置目标高度比当前高度低5米

        sp_local.timestamp = hrt_absolute_time();  // 获取当前时间戳并设置给目标位置结构体

        _trajectory_setpoint_pub.publish(sp_local); // 发布目标位置（轨迹设定点）

        ocm.position = true;  // 设置Offboard控制模式使用位置控制
        ocm.timestamp = hrt_absolute_time();  // 获取当前时间戳并设置给Offboard控制模式结构体
        _offboard_control_mode_pub.publish(ocm);  // 发布Offboard控制模式，激活Offboard控制模式

        // 设置飞行器模式为Offboard
        _command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;  // 设置命令为“设置模式”
        _command.param1 = 1.0f;  // 设置参数1，表示启用模式
        _command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;  // 设置参数2为Offboard模式
        _command.timestamp = hrt_absolute_time();  // 获取当前时间戳并设置给命令
        _vehicle_command_pub.publish(_command);  // 发布命令以切换到Offboard模式

        usleep(10000);  // 暂停10毫秒，给飞控系统时间来处理模式切换命令

        // 设置解锁命令
        _command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;  // 设置命令为“解锁”命令
        _command.param1 = 1.0f;  // 设置参数1为1，表示解锁（arming）
        _command.timestamp = hrt_absolute_time();  // 获取当前时间戳并设置给命令
        _vehicle_command_pub.publish(_command);  // 发布解锁命令

    }


    // 记录当前点的位置
    _vehicle_local_position_sub.copy(&_vehicle_local_position);
    begin_x = _vehicle_local_position.x;
    begin_y = _vehicle_local_position.y;
    begin_z = _vehicle_local_position.z;
    time_tick = hrt_absolute_time();
    // 业务逻辑
    while (!should_exit())
    {
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        if (flag == 0) // 升至5米
        {
        flag++;
        memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));

        sp_local.x = begin_x;
        sp_local.y = begin_y;
        sp_local.z = begin_z - 5;
        ocm.position = true;
        ocm.velocity = false;
        ocm.acceleration = false;
        PX4_INFO("***********************");
        PX4_INFO("pos 005 up 5m");
        PX4_INFO("***********************");
        }

        if ((flag == 1) && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad)) // 升至5米
        {
            if (flag2 == 0)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 1) && (hrt_absolute_time() - time_tick) > 1000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x + 5;
                sp_local.y = begin_y;
                sp_local.z = begin_z - 5;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                ocm.position = true;
                ocm.velocity = false;
                ocm.acceleration = false;
                time_tick = hrt_absolute_time();
                PX4_INFO("***********************");
                PX4_INFO("pos 505");
                PX4_INFO("***********************");
            }
        }
        if ((flag == 2) && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad)) // 升至5米
        {
            if (flag2 == 1)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 2) && (hrt_absolute_time() - time_tick) > 1000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x + 5;
                sp_local.y = begin_y + 5;
                sp_local.z = begin_z - 5;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                ocm.position = true;
                ocm.velocity = false;
                ocm.acceleration = false;
                time_tick = hrt_absolute_time();
                PX4_INFO("***********************");
                PX4_INFO("pos 555");
                PX4_INFO("***********************");
            }
        }
        if (flag == 3 && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad))
        {
            if (flag2 == 2)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 3) && (hrt_absolute_time() - time_tick) > 1000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x;
                sp_local.y = begin_y + 5;
                sp_local.z = begin_z - 5;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                ocm.position = true;
                ocm.velocity = false;
                ocm.acceleration = false;
                PX4_INFO("***********************");
                PX4_INFO("pos 055");
                PX4_INFO("***********************");
            }
        }
        if (flag == 4 && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad))
        {
            if (flag2 == 3)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 4) && (hrt_absolute_time() - time_tick) > 1000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x ;
                sp_local.y = begin_y ;
                sp_local.z = begin_z - 5;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                ocm.position = true;
                ocm.velocity = false;
                ocm.acceleration = false;
                PX4_INFO("***********************");
                PX4_INFO("pos 005");
                PX4_INFO("***********************");
            }
        }
        if (flag == 5 && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad))
        {
            if (flag2 == 4)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 5) && (hrt_absolute_time() - time_tick) > 5000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x;
                sp_local.y = begin_y;
                sp_local.z = begin_z - 10;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                sp_local.yaw = (float)NAN;
                sp_local.yawspeed = (float)NAN;
                ocm.position = true;
                ocm.velocity = true;
                ocm.acceleration = false;
                PX4_INFO("***********************");
                PX4_INFO("pos 00 10 up 5m");
                PX4_INFO("***********************");
            }
        }
        if (flag == 6 && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad))
        {
            if (flag2 == 5)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 6) && (hrt_absolute_time() - time_tick) > 5000000)
            {
                flag++;
                memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s));
                sp_local.x = begin_x;
                sp_local.y = begin_y;
                sp_local.z = begin_z - 5;
                sp_local.vx = (float)NAN;
                sp_local.vy = (float)NAN;
                sp_local.vz = (float)NAN;
                sp_local.acceleration[0] = (float)NAN;
                sp_local.acceleration[1] = (float)NAN;
                sp_local.acceleration[2] = (float)NAN;
                sp_local.yaw = (float)NAN;
                sp_local.yawspeed = (float)NAN;
                ocm.position = true;
                ocm.velocity = false;
                ocm.acceleration = true;
                PX4_INFO("***********************");
                PX4_INFO("pos 005 down 5m");
                PX4_INFO("***********************");
            }
        }

        if (flag == 7 && (abs(_vehicle_local_position.x - sp_local.x) < xy_rad) && (abs(_vehicle_local_position.y - sp_local.y) < xy_rad) && (abs(_vehicle_local_position.z - sp_local.z) < z_rad))
        {
            if (flag2 == 6)
            {
                flag2++;
                time_tick = hrt_absolute_time();
            }
            if ((flag2 == 7) && (hrt_absolute_time() - time_tick) > 5000000)
            {
                flag++;
                _command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
                _command.param1 = 1.0f;
                _command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
                _command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
                _command.timestamp = hrt_absolute_time();
                _vehicle_command_pub.publish(_command);
                PX4_INFO("return");
            }
        }



        // 修改主要代码
        if (ocm.position || ocm.velocity || ocm.acceleration) {
            ocm.timestamp = hrt_absolute_time();
            _offboard_control_mode_pub.publish(ocm);
            _vehicle_status_sub.copy(&_status);

            if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
                // 获取当前时间和时间步长
                const hrt_abstime now = hrt_absolute_time();
                const float dt = (_last_update != 0) ? ((now - _last_update) / 1e6f) : 0.0f;
                _last_update = now;

                // 获取当前状态
                Vector3f current_position(_vehicle_local_position.x, _vehicle_local_position.y, _vehicle_local_position.z);
                Vector3f current_velocity(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
                Vector3f current_acceleration(0.0f, 0.0f, 0.0f); // 如果有加速度数据，可以使用实际值

                // 设置目标点
                Vector3f target_position(sp_local.x, sp_local.y, sp_local.z);
                Vector3f target_velocity(sp_local.vx, sp_local.vy, sp_local.vz);

                // 初始化轨迹平滑器（如果是第一次运行）
                static bool initialized = false;
                if (!initialized) {
                    initialize_smoothing();
                    _position_smoothing.reset(current_acceleration, current_velocity, current_position);
                    initialized = true;
                }

                // 生成平滑轨迹
                PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
                _position_smoothing.generateSetpoints(
                    current_position,             // 当前位置
                    target_position,              // 目标位置
                    target_velocity,              // 目标速度
                    dt,                          // 时间步长
                    false,                       // 不强制零速度
                    smoothed_setpoints           // 输出的平滑轨迹点
                );

                // 保留原始设定点 sp_local，并定义一个新的变量，用于存储平滑后的设定点
                vehicle_local_position_setpoint_s smoothed_sp_local = sp_local;

                // 更新轨迹设定点，使用新的变量 smoothed_sp_local
                smoothed_sp_local.timestamp = now;
                // 位置设定点
                smoothed_sp_local.x = smoothed_setpoints.position(0);
                smoothed_sp_local.y = smoothed_setpoints.position(1);
                smoothed_sp_local.z = smoothed_setpoints.position(2);
                // 速度设定点
                smoothed_sp_local.vx = smoothed_setpoints.velocity(0);
                smoothed_sp_local.vy = smoothed_setpoints.velocity(1);
                smoothed_sp_local.vz = smoothed_setpoints.velocity(2);
                // 加速度设定点
                smoothed_sp_local.acceleration[0] = smoothed_setpoints.acceleration(0);
                smoothed_sp_local.acceleration[1] = smoothed_setpoints.acceleration(1);
                smoothed_sp_local.acceleration[2] = smoothed_setpoints.acceleration(2);

                // 发布平滑的轨迹设定点，而不是原始的 sp_local
                _trajectory_setpoint_pub.publish(smoothed_sp_local);


                // 输出调试信息
                PX4_INFO("------------------------------------------------------------------");
                PX4_INFO("Begin Position: x=%.3f, y=%.3f, z=%.3f",
                    (double)begin_x, (double)begin_y, (double)begin_z);
                PX4_INFO("Target Position: x=%.3f, y=%.3f, z=%.3f",
                        (double)target_position(0), (double)target_position(1), (double)target_position(2));
                PX4_INFO("Smoothed Position: x=%.3f, y=%.3f, z=%.3f",
                        (double)smoothed_setpoints.position(0),
                        (double)smoothed_setpoints.position(1),
                        (double)smoothed_setpoints.position(2));
                PX4_INFO("Current Position: x=%.3f, y=%.3f, z=%.3f",
                        (double)current_position(0), (double)current_position(1), (double)current_position(2));
                PX4_INFO("Smoothed Velocity: vx=%.3f, vy=%.3f, vz=%.3f",
                        (double)smoothed_setpoints.velocity(0),
                        (double)smoothed_setpoints.velocity(1),
                        (double)smoothed_setpoints.velocity(2));
                PX4_INFO("Current Velocity: vx=%.3f, vy=%.3f, vz=%.3f",
                        (double)current_velocity(0), (double)current_velocity(1), (double)current_velocity(2));
                PX4_INFO("------------------------------------------------------------------");
            }
        }
        usleep(100000);
        parameters_update();
    }
}

void ControlNode::parameters_update(bool force)
{
    // check for parameter updates
    if (_parameter_update_sub.updated() || force)
    {
        // clear update
        parameter_update_s update;
        _parameter_update_sub.copy(&update);

        // update parameters from storage
        updateParams();
    }
}

int ControlNode::print_usage(const char *reason)
{
    if (reason)
    {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Section that describes the provided module functionality.

This is a template for a module running as a task in the background with start/stop/status functionality.

### Implementation
Section describing the high-level implementation of this module.

### Examples
CLI usage example:
$ module start -f -p 42

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("module", "template");
    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
    PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

int control_node_main(int argc, char *argv[])
{
    return ControlNode::main(argc, argv);
}

创建 control_node.h

创建 control_node.h 文件，并将以下内容复制进去：

#pragma once
#include "commander/px4_custom_mode.h"
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
// 引用未引用的库
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>

#include <motion_planning/VelocitySmoothing.hpp> // 引入VelocitySmoothing头文件
#include <motion_planning/PositionSmoothing.hpp> // 引入PositionSmoothing头文件

using namespace time_literals;

extern "C" __EXPORT int control_node_main(int argc, char *argv[]);


class ControlNode : public ModuleBase<ControlNode>, public ModuleParams
{
public:
    ControlNode(int example_param, bool example_flag);

    virtual ~ControlNode() = default;

    /** @see ModuleBase */
    static int task_spawn(int argc, char *argv[]);

    /** @see ModuleBase */
    static ControlNode *instantiate(int argc, char *argv[]);

    /** @see ModuleBase */
    static int custom_command(int argc, char *argv[]);

    /** @see ModuleBase */
    static int print_usage(const char *reason = nullptr);

    /** @see ModuleBase::run() */
    void run() override;

    /** @see ModuleBase::print_status() */
    int print_status() override;

    // 声明initialize_smoothing函数
    void initialize_smoothing();

private:

    /**
     * Check for parameter changes and update them if needed.
     * @param parameter_update_sub uorb subscription to parameter_update
     * @param force for a parameter update
     */
    void parameters_update(bool force = false);


    DEFINE_PARAMETERS(
        (ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
        (ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
    )

    uint64_t time_tick=hrt_absolute_time();
    int flag=0,flag2=0;
    vehicle_local_position_s _vehicle_local_position;
    vehicle_status_s _status;
    vehicle_command_s _command = {};
    offboard_control_mode_s ocm{};
    position_setpoint_triplet_s _pos_sp_triplet{};
    vehicle_command_ack_s _ack{};
    vehicle_local_position_setpoint_s sp_local{};
    vehicle_attitude_setpoint_s attitude_setpoint{};
    // Subscriptions
    uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
    uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
    uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

    // Publications
    uORB::Publication<offboard_control_mode_s>        _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
    uORB::Publication<vehicle_command_s>        _vehicle_command_pub{ORB_ID(vehicle_command)};
    uORB::Publication<position_setpoint_triplet_s>        _position_setpoint_triplet_pub{ORB_ID(position_setpoint_triplet)};
    uORB::Publication<vehicle_local_position_setpoint_s>    _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};

    PositionSmoothing _position_smoothing; // 轨迹平滑器
        hrt_abstime _last_update{0};          // 上次更新时间

    };

3. 添加编译脚本配置

在 PX4-Autopilot 目录下，找到并打开 boards/px4/sitl/default.px44board 文件，添加以下一行内容：

CONFIG_MODULES_CONTROL_NODE=y

注意： 确保按正确的顺序添加此配置。错误的顺序可能导致编译失败。在某些情况下，可能需要尝试不同的位置才能确保编译通过。

二、测试

1. 编译与运行

完成上述步骤后，执行以下命令进行编译。请确保在 PX4-Autopilot 目录下运行：

make px4_sitl_default gazebo

2. 启动控制节点

在 Gazebo 仿真启动后，执行以下命令启动 control_node：

control_node start

3. 测试结果

命令执行后，无人机会进入 OFFBOARD 模式，正常起飞，并执行以下动作：

• 上升 5 米
• 飞行一个正方形路径
• 降落

程序将输出详细信息，帮助监控飞行过程。

以上就是通过 PX4 的 OFFBOARD 模式控制无人机自主飞行的步骤。通过自定义模块的实现，你可以扩展无人机的飞行控制功能，为进一步的实验和应用提供基础。