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