fcu_mission 任务规划
fcu_mission 是 fcu_core_v2 中的任务规划模块。规划飞行任务前，需获取无人机的当前位姿。
该模块首先接收飞控发送的里程计（odom）数据：
ros::Subscriber odom001=nh.subscribe<nav_msgs::Odometry>("odom_global_001", 100, odom_global001_handler);
随后进行坐标变换，并将四元数转换为姿态角：
void odom_global001_handler(const nav_msgs::Odometry::ConstPtr& odom)
{
pos_odom_001_x=(float)odom->pose.pose.position.x;// 将位置坐标转换为 FRU 坐标
pos_odom_001_y=-(float)odom->pose.pose.position.y;
pos_odom_001_z=(float)odom->pose.pose.position.z;
float quaternion_odom[4]={(float)odom->pose.pose.orientation.w,
(float)odom->pose.pose.orientation.x,
(float)odom->pose.pose.orientation.y,
(float)odom->pose.pose.orientation.z};
mavlink_quaternion_to_euler(quaternion_odom, &pos_odom_001_roll, &pos_odom_001_pitch, &pos_odom_001_yaw);
}
fcu_mission 提供四个飞行任务例程，用于演示轨迹规划。各例程均通过键盘指令启动，因此需订阅键盘指令话题：
ros::Subscriber comm=nh.subscribe<std_msgs::Int16>("/fcu_command/command", 100, cmdHandler);
键盘指令由 fcu_command.cpp 发布。每个按键对应一个 case 分支，可参照现有分支添加自定义任务的启动按键。
switch(buf[0]){
case 'p':
printf("执行路径\n");
cmd.data=0;
command.publish(cmd);
break;
case 'a':
printf("解锁\n");
cmd.data=1;
command.publish(cmd);
break;
case 'd':
printf("锁定\n");
cmd.data=2;
command.publish(cmd);
break;
case 't':
printf("起飞\n");
cmd.data=3;
command.publish(cmd);
break;
case 'l':
printf("降落\n");
cmd.data=4;
command.publish(cmd);
break;
***
fcu_mission 中的键盘指令处理函数同样使用 switch/case 语句，根据指令选择相应任务：
void cmdHandler(const std_msgs::Int16::ConstPtr& cmd){
switch(cmd->data){
case 0:
enable_path=true;
break;
***
飞行任务规划例程
绕圆飞行
if(enable_track){
theta+=M_PI/20/200;
px1=1.0*cosf(theta)+2;
py1=1.0*sinf(theta)+2;
pz1=0.6;
px2=1.0*cosf(theta+M_PI*2/6)+2;
py2=1.0*sinf(theta+M_PI*2/6)+2;
pz2=0.6;
px3=1.0*cosf(theta+M_PI*4/6)+2;
py3=1.0*sinf(theta+M_PI*4/6)+2;
pz3=0.6;
px4=1.0*cosf(theta+M_PI*6/6)+2;
py4=1.0*sinf(theta+M_PI*6/6)+2;
pz4=0.6;
px5=1.0*cosf(theta+M_PI*8/6)+2;
py5=1.0*sinf(theta+M_PI*8/6)+2;
pz5=0.6;
px6=1.0*cosf(theta+M_PI*10/6)+2;
py6=1.0*sinf(theta+M_PI*10/6)+2;
pz6=0.6;
}
将无人机放置在 theta==0 对应的初始位置，启动并建立连接，确认收到心跳包后，依次输入 a（解锁）、t（起飞）、r（绕圆飞行）；输入 s 可停止绕圆飞行。
轨迹飞行
轨迹飞行的处理方式与绕圆飞行类似，具体实现如下：
switch (path_track_status)
{
case ReadyToGoal:
switch(goal_point)
{
case 0:
SetGoal(1,0.5,0.5,0);
break;
case 1:
SetGoal(1,1.0,1.0,0);
break;
case 2:
SetGoal(1,1.5,1.0,0);
break;
case 3:
SetGoal(1,2.0,1.0,0);
break;
case 4:
SetGoal(1,2.5,1.0,0);
break;
case 5:
SetGoal(1,2.5,2.0,0);
break;
case 6:
SetGoal(1,2.0,2.0,0);
break;
case 7:
SetGoal(1,1.5,2.0,0);
break;
case 8:
SetGoal(1,1.0,2.0,0);
break;
case 9:
SetGoal(1,0.5,2.0,0);
break;
}
path_track_status = ExecutingGoal;
break;
case ExecutingGoal:
if(IsReachGoal(1,0.1))
{
path_track_status = ReadyToGoal;
goal_point++;
break;
}
// std::cout << "Not Finish yet..." << std::endl;
break;
将无人机放置在 goal_point==0 对应的初始位置，启动并建立连接，确认收到心跳包后，依次输入 a（解锁）、t（起飞）、p（执行路径）；输入 s 可停止执行。
定点飞行
本节为键盘控制例程，目标位置由起飞位置与设定偏移量相加得到。RViz 插件航点列表中填写的是所用全局坐标系下的绝对坐标，具体操作参见 rviz_plugin 的使用。
switch(enable_pos){
case 0:
px1=pos_takeoff_001_x; py1=pos_takeoff_001_y; pz1=0.0f;
px2=pos_takeoff_002_x; py2=pos_takeoff_002_y; pz2=0.0f;
px3=pos_takeoff_003_x; py3=pos_takeoff_003_y; pz3=0.0f;
px4=pos_takeoff_004_x; py4=pos_takeoff_004_y; pz4=0.0f;
px5=pos_takeoff_005_x; py5=pos_takeoff_005_y; pz5=0.0f;
px6=pos_takeoff_006_x; py6=pos_takeoff_006_y; pz6=0.0f;
break;
case 1:
px=1.0;
py=1.0;
pz=1.0;
px1=pos_takeoff_001_x+px; py1=pos_takeoff_001_y+py; pz1=pz;
px2=pos_takeoff_002_x+px; py2=pos_takeoff_002_y+py; pz2=pz;
px3=pos_takeoff_003_x+px; py3=pos_takeoff_003_y+py; pz3=pz;
px4=pos_takeoff_004_x+px; py4=pos_takeoff_004_y+py; pz4=pz;
px5=pos_takeoff_005_x+px; py5=pos_takeoff_005_y+py; pz5=pz;
px6=pos_takeoff_006_x+px; py6=pos_takeoff_006_y+py; pz6=pz;
break;
放置无人机，启动并建立连接，确认收到心跳包后，依次输入 a（解锁）、t（起飞），再输入 1 至 4 中的目标点编号，执行对应的定点飞行任务。
定点飞行通过 fcu_command 中的 enable_pos 指令选择目标点。pos_takeoff_001_x 和 pos_takeoff_001_y 记录起飞位置，px 和 py 表示相对于起飞点的偏移量。将偏移量与起飞位置相加后，得到全局坐标系中的绝对目标位置；最终发布的目标点均为绝对坐标。
追踪飞行
追踪飞行包括前视追踪和下视追踪，分别适用于搭载机载电脑并配备前视摄像机或下视摄像机的无人机。机载电脑需运行 fcu_core_v2 和 ArUco 检测程序。
在远程电脑上按下对应按键，即可向飞控发送追踪指令。
飞控将消息转发至机载电脑，由机载电脑启动追踪并下发追踪控制指令。具体流程如下图所示：
定时器执行函数
fcu_mission 通过定时器周期性发布规划目标点的位置和姿态数据。
ros::Timer timer_mission_001 = nh.createTimer(ros::Duration(0.1),execute_mission_001,false);
定时器周期为 100 ms，每个周期调用一次 execute_mission_001 函数：
void execute_mission_001(const ros::TimerEvent &event){
if(get_pos_cmd){
return;
}
if(set_goal&&!use_goal_001){
return;
}
// 发布 mission 数据
mission_001.layout.dim.push_back(std_msgs::MultiArrayDimension());
mission_001.layout.dim[0].label = "mission_001";
mission_001.layout.dim[0].size = 11;
mission_001.layout.dim[0].stride = 1;
mission_001.data.resize(11);
mission_001.data[0]=yaw;//rad
mission_001.data[1]=yaw_rate;//rad/s
mission_001.data[2]=px1;//x
mission_001.data[3]=py1;//y
mission_001.data[4]=pz1;//z
mission_001.data[5]=vx;//vx
mission_001.data[6]=vy;//vy
mission_001.data[7]=vz;//vz
mission_001.data[8]=ax;//ax
mission_001.data[9]=ay;//ay
mission_001.data[10]=az;//az
mission_pub_001.publish(mission_001);
use_goal_001=false;
}
该函数负责填充并发布任务数据。publish 将规划目标点通过 ROS 话题发送至 fcu_bridge，再由 fcu_bridge 转发至飞控。