PX4无人机飞控开发——第2篇：RTL返航模式优化

PX4无人机飞控开发——第2篇：RTL返航模式优化

大家好我是Jones,写博客记录一下工作的痕迹，同时也对工作做一个总结，才疏学浅，难免会有很多纰漏，还请大家批评指正!

PX4无人机飞控开发系列

第1篇：仿真工具介绍
第2篇：RTL返航模式优化
第3篇：室内定点之光流一：PID位置控制
第4篇：室内定点之光流二：EKF融合
第5篇：GAZEBO仿真

---

---

前言

我们重新对返航什么时候对头，什么时候队尾进行定义，当前距离是否超过返航距离，当前高度是否超过返航高度进行判断，为了飞行安全，对无人机返航模式进行重新梳理，设计其功能，画出逻辑流程图，代码实现，并仿真飞行验证。

---

---

一、原RTL可能存在的几个问题

1.原RTL返航过程中，一边飞行一边转航向，对于大轴距无人机来说，飞行动作较危险，动静非常大，我们将转航向和平移分开，飞机在悬停的时候转航向。

2.原RTL返航在小于返航距离的情况下，也旋转航向进行对头。近距离对头再对尾没有必要，浪费较多飞行时间。

3.原RTL返航，不是每一次返航降落都是对尾，对于习惯于对尾飞行的人来说，需要去重新摆正飞机，或是出现问题后由于不是队尾易出现操作上的困难。

.

二、优化的RTL返航

1.优化的逻辑流程

2.代码优化

我们在rtl.h里添加两个悬停阶段，用于对头队尾。

RTL代码目录：Firmware/src/modules/navigator/rtl.h

enum RTLState {
		RTL_STATE_NONE = 0,
		RTL_STATE_CLIMB,
		RTL_STATE_RETURN,
		RTL_STATE_TRANSITION_TO_MC,
		RTL_STATE_DESCEND,
		RTL_STATE_LOITER,
		RTL_STATE_LAND,
		RTL_STATE_LANDED,
            RTL_STATE_LOITER1,//悬停对头
            RTL_STATE_LOITER2,//悬停队尾
	} _rtl_state{RTL_STATE_NONE};
	

RTL代码目录：Firmware/src/modules/navigator/rtl.cpp

悬停对头：

case RTL_STATE_LOITER1:{
        if(home_dist > _param_rtl_min_dist.get()){
            _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
            _mission_item.lat = gpos.lat;
            _mission_item.lon = gpos.lon;
            _mission_item.altitude = return_alt;
            _mission_item.altitude_is_relative = false;
            _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
            _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
            _mission_item.time_inside = 1.0f;
            _mission_item.autocontinue = true;
            _mission_item.origin = ORIGIN_ONBOARD;
            mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter1_1 %.1fs",
                             (double)get_time_inside(_mission_item));
        }else{
            _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
            _mission_item.lat = gpos.lat;
            _mission_item.lon = gpos.lon;
            _mission_item.altitude = gpos.alt;
            _mission_item.altitude_is_relative = false;
            _mission_item.yaw = home.yaw;
            _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
            _mission_item.time_inside = max(_param_land_delay.get(), 0.0f);
            _mission_item.autocontinue = true;
            _mission_item.origin = ORIGIN_ONBOARD;
            mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter1_2 %.1fs",
                             (double)get_time_inside(_mission_item));
        }
            break;
            

悬停队尾：

case RTL_STATE_LOITER2:{
            _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
            _mission_item.lat = gpos.lat;
            _mission_item.lon = gpos.lon;
            _mission_item.altitude = return_alt;
            _mission_item.altitude_is_relative = false;
            _mission_item.yaw = home.yaw;
            _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
            _mission_item.time_inside = 1.0f;
            _mission_item.autocontinue = true;
            _mission_item.origin = ORIGIN_ONBOARD;
            mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter2 %.1fs",
                             (double)get_time_inside(_mission_item));
            break;
            

3.仿真验证

通过编译后，进行仿真验证，可实现逻辑流程图中的返航功能。

点击跳转->B站：PX4无人机飞控开发_02RTL返航模式优化

.

总结

通过对返航模式的优化，能够使返航功能更安全可靠，减少了一边飞行一边转航向的危险，远距离返航是对头飞行，能够通过相机看到前飞航线状态，能有效避开障碍和进行前方避障功能；返航后进行队尾，减少了摆放飞机的操作，队尾也提高了操作的可靠性；近距离不进行爬升减少相应的飞行时长，近距离返航降落是确保了起飞点周围满足飞行条件的，通过实际测试，该优化后的返航模式是安全可靠合理的，如有更好的建议可以一起探讨哦！！！