px4无人机编队学习记录-单机控制接口
笔者使用的源码及学习的内容来自
https://www.bilibili.com/video/BV1rh4y1K7No/?share_source=copy_web&vd_source=be33b1553b08cc7b94afdd6c8a50dc5a
源码下载链接在这个视频的简介里。
这位up在csdn有同名账号，超维空间科技，大佬还有很多其他精彩的博文，欢迎大家去关注学习
笔者是直接下载的源码，过程跟着过了一遍。
一 添加代码
PX4-Autopilot/src/modules/control_node里新建四个文件,名称及内容分别如下
CMakeLists.txt
px4_add_module(
MODULE modules__control_node
MAIN control_node
SRCS
control_node.cpp
)
---------------------------------------------------------------------------------------------------------------------------
control_node.cpp
#include "control_node.h"
#include
#include
#include
#include
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)
{
}
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())
{
_vehicle_status_sub.copy(&_status);
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");
break;
}
_command.target_system = _status.system_id;
_command.target_component = _status.component_id;
ocm.timestamp = hrt_absolute_time();
_vehicle_local_position_sub.copy(&_vehicle_local_position);
sp_local.x=_vehicle_local_position.x;
sp_local.y=_vehicle_local_position.y;
sp_local.z=_vehicle_local_position.z-5;
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local);
ocm.position=true;
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
_command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
_command.param1=1.0f;
_command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);
usleep(10000);
_command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
_command.param1 = 1.0f;
_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("pos 005");
}
if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)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("pos 505");
}
}
if((flag==2)&&(abs(_vehicle_local_position.x-sp_local.x)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("pos 555");
}
}
if(flag==3&&(abs(_vehicle_local_position.x-sp_local.x)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;
sp_local.yaw=1;
sp_local.yawspeed=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yaw");
}
}
if(flag==4&&(abs(_vehicle_local_position.heading-sp_local.yaw)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;
sp_local.yaw=(float)NAN;
sp_local.yawspeed=1;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yawspeed");
}
}
if(flag==5)
{
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=(float)NAN;
sp_local.y=(float)NAN;
sp_local.z=begin_z-5;
sp_local.vx=1;
sp_local.vy=0;
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("velocity");
}
}
if(flag==6)
{
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=(float)NAN;
sp_local.y=(float)NAN;
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]=0;
sp_local.acceleration[1]=1;
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("acceleration");
}
}
if(flag==7)
{
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)
{
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local);
// PX4_INFO("sp_local.x=%lf\n",(double)sp_local.x);
// PX4_INFO("sp_local.y=%lf\n",(double)sp_local.y);
// PX4_INFO("sp_local.z=%lf\n",(double)sp_local.z);
// PX4_INFO("sp_local.vx=%lf\n",(double)sp_local.vx);
// PX4_INFO("sp_local.vy=%lf\n",(double)sp_local.vy);
// PX4_INFO("sp_local.vz=%lf\n",(double)sp_local.vz);
}
}
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
#pragma once
#include "commander/px4_custom_mode.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace time_literals;
extern "C" __EXPORT int control_node_main(int argc, char *argv[]);
class ControlNode : public ModuleBase, 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;
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) _param_sys_autostart, /**< example parameter */
(ParamInt) _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_pub{ORB_ID(offboard_control_mode)};
uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication _position_setpoint_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
};
---------------------------------------------------------------------------------------------------------------------------
Kconfig
menuconfig MODULES_CONTROL_NODE
bool "control_node"
default n
---help---
Enable support for control_node
二 部分代码功能理解
2.1进入offboard模式并解锁
control_node.cpp中：
if((_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)&&(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED))
{ //判断是否成功切换到offboard并且成功解锁
PX4_INFO("in offboard and armed");
break;
}在这段代码中，有一个条件判断语句（if语句），用于检查两个条件是否同时满足。这些条件与无人机（或类似的飞行器）的导航状态和武装状态有关。让我们逐步解释这段代码：
-
条件判断 (
if语句): 这段代码使用if语句来检查两个条件是否同时为真。只有当两个条件都满足时，代码块内的语句才会执行。 -
第一个条件 (
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD): 这个条件检查无人机的导航状态（nav_state）是否为NAVIGATION_STATE_OFFBOARD。在这种状态下，无人机通常接受地面站或远程系统的控制指令，而不是自动执行预设任务。 -
第二个条件 (
_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED): 这个条件检查无人机的武装状态（arming_state）是否为ARMING_STATE_ARMED。在“武装”状态下，无人机的飞行系统是活动的，可以响应飞行指令，比如起飞和移动。 -
代码块: 如果以上两个条件都满足，代码块内的语句将被执行。在这个例子中，它执行了一个打印信息的操作（
PX4_INFO("in offboard and armed")），向用户显示无人机目前处于外部控制（offboard）模式，并且已经被武装。 -
break语句: 如果条件满足，break语句会执行，这通常意味着跳出当前的循环或停止当前的流程。
简而言之，这段代码是用来检查无人机是否已经成功切换到外部控制模式并且被武装。如果这两个条件都满足，它会显示一条信息，并执行后续的break语句，跳出这个if循环，否则对无人机进行切换。
切换功能通过command命令：
_command.target_system = _status.system_id;
_command.target_component = _status.component_id;将命令对象的目标系统设置为状态对象中的系统ID。这意味着命令将被发送到这个特定的系统。
设置命令对象的目标组件为状态对象中的组件ID。命令就会被定向到系统中的特定组件。
----------------------------------------------------------------------------------------------------------------------------
ocm.timestamp = hrt_absolute_time();这行代码给一个对象设置一个时间戳，这个时间戳是通过hrt_absolute_time()函数获得的。该函数通常返回当前的绝对时间。
-------------------------------------------------------------------------------------------------------------------------
然后是发布坐标代码，新版本（什么版本的算新这个笔者暂时不清楚）的固件下，要切换到offboard模式，必须要先发布期望的位置
_vehicle_local_position_sub.copy(&_vehicle_local_position);
sp_local.x=_vehicle_local_position.x;
sp_local.y=_vehicle_local_position.y;
sp_local.z=_vehicle_local_position.z-5;
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local); ocm.position=true;
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);ocm是offboard的心跳包，需要以最低2hz的频率进行发布，才不会退出offboard模式
_command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
_command.param1=1.0f;
_command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);param1,主模式，直接设置成1；param2，从模式，设置成offfboard。
_command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
_command.param1 = 1.0f;
_command.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(_command);通过ARM_DISARM进行解锁；param1 =1 即为解锁，=0为加锁；
2.2 记录当前点的位置
//记录当前点的位置
_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;copy(&_vehicle_local_position) 对本地位置进行订阅，begin_x/y/z 三个变量对起飞之前的位置进行记录，为了对无人机位置的漂移进行抵消。
2.3 主要实现控制的部分
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("pos 005");memset(&sp_local, 0, sizeof(vehicle_local_position_setpoint_s))；
这行代码使用memset函数初始化sp_local变量。sp_local是vehicle_local_position_setpoint_s类型的一个实例，这个函数将其所有的位设置为0。这通常用于清除或重置结构体的数据。
sp_local.x=begin_x;
sp_local.y=begin_y;
sp_local.z=begin_z-5;
x,y设置成初始位置，z 高度设置成5米，这里涉及到“被动地”的概念。
被动地坐标系（通常称为“地心地固坐标系”，英文为 Earth-Centered, Earth-Fixed coordinate system，简称 ECEF）是一种用于描述地球上位置的坐标系统。在这个系统中，坐标的原点位于地球的质心，坐标轴与地球的自转轴和赤道平面固定。
在被动地坐标系中：
- X轴：通常指向经过格林威治的赤道点，即格林威治子午线和赤道的交点。
- Y轴：与X轴垂直，同样位于赤道平面上，指向东经90度的方向。
- Z轴：与地球的自转轴一致，指向北极。
这个坐标系在地理信息系统（GIS）、卫星导航和天文学中广泛使用，因为它提供了一个稳定的框架，可以用于精确地描述地球表面的位置。与地球表面的地理坐标（经度和纬度）相比，ECEF坐标系是一个笛卡尔坐标系统，提供了一个更直观的方式来表示三维空间中的位置。在ECEF坐标系中，一个点的位置是通过一个三维向量表示的，该向量从地球中心指向那个点。这与其他一些常见的坐标系统（如地理坐标系统，其中位置由经度、纬度和高度表示）有所不同。
在被动低的坐标系（通常指向下为正方向的坐标系）中，sp_local.z = begin_z - 5; 这行代码的效果会有所不同。在这种坐标系中，如果 begin_z 代表当前高度，减去 5 实际上意味着增加高度而非降低。
例如，如果在被动低的坐标系中 begin_z 是 10 米（向下为正，所以这表示距离参考点下方 10 米），执行 sp_local.z = begin_z - 5; 后，sp_local.z 将变为 5 米，这实际上是向上移动了 5 米（因为从距离参考点下方 10 米变成了下方 5 米）。
总结来说，在向下为正的坐标系中，减去一个值实际上是使物体向上移动，与传统的向上为正的坐标系正好相反。所以在这种情况下，sp_local.z = begin_z - 5; 会使物体上升 5 米。
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("pos 005");-
ocm.position = true;设置了一个名为ocm的对象的position属性为true，意味着位置控制是激活的。 -
ocm.velocity = false; 将ocm对象的velocity（速度）属性设置为false，表示不进行速度控制。 -
ocm.acceleration = false;类似地，这行代码将ocm对象的acceleration（加速度）属性设置为false，意味着加速度控制未激活。 -
PX4_INFO("pos 005");这行代码打印一条信息到日志或控制台。文本"pos 005"可能是一种标记或提示，用于调试或跟踪执行到这个点的情况。
2.3.1 flag =1
if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)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("pos 505");
}
} 条件判断:
if((flag==1)&&(abs(_vehicle_local_position.x-sp_local.x)这个条件语句检查几个条件：flag 是否等于 1，无人机的当前位置（_vehicle_local_position）是否在以 sp_local 为中心的一个矩形区域内（这个区域在XY平面上的半径是 xy_rad，在Z轴上的半径是 z_rad）
if(flag2==0)
{
flag2++;
time_tick=hrt_absolute_time();
}if(flag2==0) 如果 flag2 等于0，这通常表示一个特定的状态或阶段的开始；
flag2++; flag2 增加 1，可能用来标记已经进入了新的状态或阶段。
time_tick=hrt_absolute_time(); 记录当前时间，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("pos 505");
} if((flag2==1)&&(hrt_absolute_time()-time_tick)>1000000) 这个条件检查 flag2 是否等于 1，并且自上次记录时间以来是否已经过了1,000,000微秒（1秒)
如果是，执行以下操作：
flag++; 增加 flag 的值，可能用来进入下一个状态或阶段。
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s)); 重置 sp_local 结构体，将其所有的位设置为0。
sp_local.x=begin_x+5; x坐标增加了5,表示往北方向飞行5米。
sp_local.y=begin_y;
sp_local.z=begin_z-5; z坐标降低了5（这里表示上升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;
将速度 (vx, vy, vz) 和加速度 (acceleration[0], acceleration[1], acceleration[2]) 设置为 NAN（不是数字），表示这些值当前不可用或不确定。
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false; 设置 ocm 对象的 position, velocity, acceleration 属性，表示当前只激活位置控制。
time_tick=hrt_absolute_time(); 重新记录当前时间到 time_tick。
PX4_INFO("pos 505"); 输出信息 "pos 505"，表示程序已经到达这个阶段。
2.3.2 flag =2
if((flag==2)&&(abs(_vehicle_local_position.x-sp_local.x)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("pos 555");
} if((flag==2)&&(abs(_vehicle_local_position.x-sp_local.x) if((flag2==1)&&(hrt_absolute_time()-time_tick)>1000000) 这个条件检查 flag2 是否等于 1，并且自上次记录时间以来是否已经过了1,000,000微秒（1秒)
如果是，执行以下操作：
if(flag2==1)
{
flag2++;
time_tick=hrt_absolute_time();
}if(flag2==1) 如果 flag2 等于1，这通常表示一个特定的状态或阶段的开始；
flag2++; flag2 增加 1，可能用来标记已经进入了新的状态或阶段。
time_tick=hrt_absolute_time(); 记录当前时间，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("pos 555");
}和上面一段类似，也可以过一遍
if((flag2==2)&&(hrt_absolute_time()-time_tick)>1000000) 这个条件检查 flag2 是否等于 2，并且自上次记录时间以来是否已经过了1,000,000微秒（1秒)
如果是，执行以下操作：
flag++; 增加 flag 的值，可能用来进入下一个状态或阶段。
memset(&sp_local,0,sizeof(vehicle_local_position_setpoint_s)); 重置 sp_local 结构体，将其所有的位设置为0。
sp_local.x=begin_x+5; x坐标增加了5,表示往北方向飞行5米,但此处是从begin_x 加上5，和flag=1时一样的，所以x方向并没有移动。
sp_local.y=begin_y+5; y坐标增加5，表示往东飞行5米。
sp_local.z=begin_z-5; z坐标降低了5（这里表示上升5米）但此处是从begin_z 加上5，和flag=1时一样的，所以z方向并没有移动。。
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;
将速度 (vx, vy, vz) 和加速度 (acceleration[0], acceleration[1], acceleration[2]) 设置为 NAN（不是数字），表示这些值当前不可用或不确定。
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false; 设置 ocm 对象的 position, velocity, acceleration 属性，表示当前只激活位置控制。
time_tick=hrt_absolute_time(); 重新记录当前时间到 time_tick。
PX4_INFO("pos 505"); 输出信息 "pos 555"，表示程序已经到达这个阶段。
2.3.3 flag =3
if(flag==3&&(abs(_vehicle_local_position.x-sp_local.x)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;
sp_local.yaw=1;
sp_local.yawspeed=(float)NAN;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yaw");
}
} 无人机保持在x+5,y+5,z-5的位置，即往北5米，往东5米，高度5米。
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;
将速度 (vx, vy, vz) 和加速度 (acceleration[0], acceleration[1], acceleration[2]) 设置为 NAN（不是数字），表示这些值当前不可用或不确定。
令无人机维持在当前位置，并且设置了速度与加速度，再去调整偏航角。
sp_local.yaw=1; 设置1弧度的偏航角
PX4_INFO("yaw"); 输出信息 "yaw"，表示程序已经到达这个阶段。
2.3.4 flag=4
if(flag==4&&(abs(_vehicle_local_position.heading-sp_local.yaw)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;
sp_local.yaw=(float)NAN;
sp_local.yawspeed=1;
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false;
PX4_INFO("yawspeed");
}
} sp_local.yawspeed=1; 对偏航角速度进行控制
ocm.position=true;
ocm.velocity=false;
ocm.acceleration=false; 进行位置控制时需要这么设置。
PX4_INFO("yawspeed"); 输出信息 "yaw"，表示程序已经到达这个阶段。
2.3.5 flag=5
if(flag==5)
{
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=(float)NAN;
sp_local.y=(float)NAN;
sp_local.z=begin_z-5;
sp_local.vx=1;
sp_local.vy=0;
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("velocity");
}
}sp_local.x=(float)NAN;
sp_local.y=(float)NAN;
sp_local.z=begin_z-5;
进行速度控制，x,y设置成NAN，高度不变。
sp_local.vx=1; 往北的速度设置成1米/秒
sp_local.vy=0; 往东的速度为0米/秒
sp_local.vz=(float)NAN;
ocm.position=true;
ocm.velocity=true; 要对速度进行控制，此处需设置为true
ocm.acceleration=false;
PX4_INFO("velocity"); 输出信息 "velocity"，表示程序已经到达这个阶段。
2.3.6 flag=6
if(flag==6)
{
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=(float)NAN;
sp_local.y=(float)NAN;
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]=0;
sp_local.acceleration[1]=1;
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("acceleration");
}
}sp_local.acceleration[0]=0; 往北的加速度为0m/s
sp_local.acceleration[1]=1; 往东的加速度为1m/s
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("acceleration"); 输出信息 "accleration"，表示程序已经到达这个阶段。
2.3.7 flag=7
if(flag==7)
{
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");
}
}_command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
_command.param1=1.0f; param1,主模式，直接设置成1
_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"); 打印信息“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)
{
sp_local.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(sp_local);
// PX4_INFO("sp_local.x=%lf\n",(double)sp_local.x);
// PX4_INFO("sp_local.y=%lf\n",(double)sp_local.y);
// PX4_INFO("sp_local.z=%lf\n",(double)sp_local.z);
// PX4_INFO("sp_local.vx=%lf\n",(double)sp_local.vx);
// PX4_INFO("sp_local.vy=%lf\n",(double)sp_local.vy);
// PX4_INFO("sp_local.vz=%lf\n",(double)sp_local.vz);
}
}
usleep(100000);
parameters_update();
}
}if(ocm.position||ocm.velocity||ocm.acceleration) 这个条件判断检查是否有位置、速度或加速度的控制指令。
_offboard_control_mode_pub.publish(ocm); 发布离线控制模式（Offboard Control Mode）的指令。
_vehicle_status_sub.copy(&_status); 从状态订阅中复制飞行器当前的状态到_status变量。
if(_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD) 检查飞行器的导航状态是否为“离线”模式。
_trajectory_setpoint_pub.publish(sp_local); 发布轨迹设定点，这些是无人机飞行的目标位置或者路径点。
parameters_update(); 更新飞行参数，这可能包括从某个源（如地面控制站）读取新的飞行参数。
需要关注的是 sp_local 这个机构体里面变量的赋值，比如 ocm.position ocm.velocity 和ocm.accleration需不需要设置为true ; sp_local.x，sp_local.y，sp_local.z，sp_local.vx，sp_local.vy，sp_local.vz需不需要设置为NAN。
三 仿真
3.1 添加仿真脚本
打开default.px4oard , 添加语句如下
CONFIG_MODULES_CONTROL_NODE=y 
3.2 执行仿真命令
进入PX4_Autopilot文件夹，右键在终端打开，第一次编译先清除以前编译过产生的文件，使用语句如下
make distclean回车键运行
执行以下代码启动仿真,
make px4_sitl_default gazebo接着启动Qgc地面站
点击左上角
弹出Select Tool 窗口，选择Application Settings
选用 虚拟游戏手柄，否则等会运行可能会报错 遥控器丢失。
这是我没选择用虚拟游戏手柄时，运行的结果
选用虚拟游戏手柄后，点击左上角返回，
鼠标左键点击刚才启动仿真后的终端的左下角，
按一下回车键，可以看到出现了pxh>
启动对应的control_node.cpp文件，命令如下
control_node start3.3 仿真结果视频
单机控制接口仿真放大版