基于机载PC的在线控制

注意:本教程适用于所有Kerloud VTOL产品

机载计算机使用户能够实现更复杂的任务,例如轨迹跟踪、路径规划、避障等,这些是通过offboard控制和自定义的mavros 网关实现的。

软件包信息

客户受邀加入我们在 https://gitee.com/cloudkernel-tech/offboard_kerloudvtol 中的私有资源库,我们在其中维护一个offboard控制软件包。该软件包为用户提供了一个起始模板,用于可利用支持的API展开应用开发。

该资源库下存在几个分支:

  • dev_kerloudvtol: 这是一个容易掌握的基础分支,它提供了解项目框架的初始视角,并实现Kerloud VTOL 的常规航点任务。

  • demo_fieldtest: 这是一个具备常规航点任务完整配置的分支,可直接用于实际飞行中

  • demo_vtol_guidance: 这是一个面向高级用户的分支,具备所有功能:位置控制、速度控制、消息记录等。该分支可在提供的SITL环境中进行测试以进一步理解,但不可直接用于飞行测试。

关键函数

以下关键函数值得重点关注(参考demo_fieldtest分支):

  • updatePositionSetpoint():该函数更新所有飞行模式(多旋翼、固定翼和过渡)的位置设定值。

  • updateTransBehaviorStatus(): 该函数处理向前和向后转换的状态机,为转换过程生成位置和偏航设定值。

  • updateTransBehaviorStatus(): 该函数请求并触发飞控的向前转换,且只能在多旋翼模式下调用。

  • requestForwardTransition(): 该函数请求并触发飞控的向前转换,且只能在固定翼模式下调用。

  • constructLocalPosTargetForCustomizedGuidance():该函数为固定矢量位置引导模式构造一个原始目标位置(对应于主题mavros/setpoint_raw/local)。无人机将按照L1制导律飞行到目标位置,到达目标位置后固定航向。当构建新的位置目标时,固定矢量制导状态将被解除。

  • constructVelocityTargetForCustomizedGuidance():该函数为速度引导模式构造一个原始目标位置(对应于主题mavros/setpoint_raw/local)。无人机将追踪请求的前进速度、倾斜角、本地Z坐标及偏航设定点。请注意:默认情况下方向舵控制处于禁用状态。

基本代码讲解

提供的off_mission软件包中的示例程序执行航点任务。航点定义在src/off_mission/launch/waypoints_xyzy.yaml路径下的yaml文件中,带有 ENU 坐标和相应的偏航值。该软件包支持 软件在环仿真,因此用户可在实际飞行前测试他们的程序。

为了清晰起见,我们按如下顺序讲解main()函数:

  1. 订阅、发布及服务的声明

// note: only part of program is shown here for simplicity
/*subscriptions, publications and services*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 5, state_cb);

//subscription for vehicle VTOL state
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>
        ("mavros/extended_state", 2, extendedstate_cb);

//subscription for local position
ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 5, local_pose_cb);

ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>("mavros/rc/in", 5, rc_input_cb);

//publication for local position setpoint (ENU position)
ros::Publisher local_pos_sp_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 5);

我们通常在main程序开始时声明ROS订阅、发布和服务,要想获取状态信息或发送命令,可以轻松利用mavros包中的各种主题或服务,详细信息请参见:http://wiki.ros.org/mavros。例如,要获取无人机的本地位置信息,需订阅mavros/setpoint_position/local主题。请注意,该坐标是在ENU(东-北-上)坐标系中定义的。

  1. 加载航点数据

// load yaml files
XmlRpc::XmlRpcValue wp_list;
private_nh.getParam("waypoints",wp_list);
initTagetVector(wp_list);
ROS_INFO("waypoint yaml loaded!");

航点数据由参数管理器加载,并被初始化为航点目标。

  1. SITL仿真环境下解锁飞机

if (simulation_flag == 1)
{
    //set offboard mode, then arm the vehicle
    if( current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_request > ros::Duration(5.0))){
        if( set_mode_client.call(offb_set_mode) &&
            offb_set_mode.response.mode_sent){
            ROS_INFO("Offboard mode enabled");
        }
        last_request = ros::Time::now();
    } else {
        if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
    }
}

实际飞行中,我们简单地解锁无人机、切换到OFFBOARD模式即可开始任务。而在SITL模式下,我们必须通过服务mavros/cmd/arming来解锁无人机。

  1. 处理转换状态、更新目标位置和偏航设定点,以及航路点切换逻辑。该代码为转换行为提供了状态机,以确保安全。

updateTransBehaviorStatus();

updatePositionSetpoint();

updateWaypointIndex();

如何使用

机载电脑~/src/catkinws_vtol目录下提供了一个模板ROS工作区,由三个包组成:mavlink、mavros和offboardcontrol。mavlink和mavros是 应用程序设计接口(API) 章节中描述的自定义版本。

仿真测试

典型的仿真测试流程如下:

  1. 启动提供的SITL环境,如 软件在环仿真 章节中所述:

cd <sitl-space directory> \
&& bash sitl_run.sh $PWD/bin/px4 none gazebo standard_vtol
  1. 启动mavros节点:

cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch mavros px4.launch fcu_url:=udp://:14540@localhost:14557
  1. 加载包含仿真设置的offboard控制节点:

cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch off_mission off_mission.launch simulation_flag:=1

实际飞行

实际飞行场景比上面的模拟测试更加复杂,因为用户还须考虑必要的硬件因素。建议工作流程如下:

  1. 为第一次飞行校准所有传感器,包括加速度计、陀螺仪、磁力计和空速。

  2. 确保飞行区空旷且无电磁干扰。使用QGC检查GPS状况,确保可用卫星数至少在11颗以上,GPS质量足够。

  3. 准备好遥控器,确保各杆量在初始位置,分配返航开关。

  4. 手机设置wifi热点,按照 供电及编程接口 章节中的说明远程登录机载电脑。

  5. 切换到offboard控制包的目标分支,为您的任务正确编辑航路点定义文件。如有必要,可使用catkin_build命令重建工作区。

  6. 与您的团队成员就任务和可能的故障进行沟通。飞手应随时待命,保证无人机的安全。

  7. 通过命令远程启动节点:

# terminal 1: launch the mavros node
cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch mavros px4.launch fcu_url:=/dev/ttyPixhawk:921600

# terminal 2: launch the offboard control node
cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch off_mission off_mission.launch simulation_flag:=0
  1. 温馨提醒:注意安全,人身安全始终高于飞机。

Caution

用户应在实际飞行前严格测试他们的代码!

演示

下面的视频为一个仿真演示。无人机首先起飞并飞过几个航路点。offboard控制器将在第三个航点激活固定矢量引导模式。无人机到达第3个航点3秒后,offboard控制将激活速度引导模式。最后,offboard控制被QGC的返航指令打断,无人机最终飞回原位。请注意,模拟是使用我们软件包的demo_vtol_guidance分支进行的。