PX4飞控之自主返航(RTL)控制逻辑

本文基于PX4飞控1.5.5版本,分析导航模块中自护返航模式的控制逻辑和算法。
自主返航模式和导航中的其他模式一样,在Navigator_main函数中一旦触发case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:任务指令,导航模式_navigation_mode = &_rtl;即进入自主返航模式。依次执行也分为初始化函数RTL::on_activation()、主函数RTL::on_active()、退出函数RTL::on_inactive()。
1.首先是初始化函数的控制逻辑:on_activation()
(1)将当前位置设为_mission_item,再赋值给当前任务航点。

set_current_position_item(&_mission_item);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

保证第一个航点为当前位置,从而实现爬升等一系列动作。
(2)根据当前位置到home点的距离,当前高度等参数选择进入哪种子模式。
①如果已经落地,则进入落地状态

_rtl_state = RTL_STATE_LANDED;

②如果距离home点比较远,同时当前高度小于返航高度,则先爬升

_rtl_state = RTL_STATE_CLIMB;

③其他情况下,直接以当前高度飞往home点_rtl_state = RTL_STATE_RETURN;
注:主要针对当前高度大于返航高度的情况
2.主函数on_active()
主函数的功能在于返航状态切换,一个子返航模式完成后,进入下一个模式,顺序进行。然后设定对应的任务航点信息set_rtl_item(),然后进行航点位置控制。
RTL控制逻辑:
(1)爬升模式RTL_STATE_CLIMB
从当前位置爬升到指定高度climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();,经纬度不变。任务类型为航点任务_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
航点:指定返航高度、当前经纬度、不进行航向控制
(2)返回模式RTL_STATE_RETURN
在指定返回高度上到达home点上方
当距离home点的距离home_dist < _param_rtl_min_dist(默认5m),航向转为home点初始航向;
如果距离较远,同时上一航点有效,则航向为从上一航点指向home点;上一航点无效则航向从当前位置指向home点。

if (home_dist < _param_rtl_min_dist.get()) {
    _mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
    if (pos_sp_triplet->previous.valid) {
    /* if previous setpoint is valid then use it to calculate heading to home */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
        _mission_item.lat, _mission_item.lon);
    } else {
    /* else use current position */
        _mission_item.yaw = get_bearing_to_next_waypoint(
        navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
        _mission_item.lat, _mission_item.lon);
    }
}

航点:home点经纬度,返航高度,进行航向控制
(3)RTL_STATE_DESCEND
降落,home点经纬度,高度为指定的落地高度_param_descend_alt

_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

_param_land_delay如果在-0.1到0.1之间,则悬停loiter;否则直降降落landing(默认为-1,直接降落)
航点:home点经纬度,高度:落地高度QGC默认2.5m,航向为home点的原始航向
(4)RTL_STATE_LOITER
loiter:时间航向都不变,只是具有一定的悬停时间
在descend的航点处悬停一定时间,航向为home点的原始航向
loiter之后再land,
在land之前都是位置航点控制_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
(5)RTL_STATE_LAND
land:经纬度为home点的经纬度,高度为0(海拔)set_land_item
航点:航向为home处的原始航向,经纬度为home点高度,高度为海拔0m,任务模式为NAV_CMD_LAND,参看pos控制
在is_mission_item_reached中NAV_CMD_LAND
返回 _navigator->get_land_detected()->landed,因此在此处进行是否已落地检测,落地后即为真,从而进入下一个任务landed

  • 6
    点赞
  • 25
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
PX4飞控中,PWM控制代码主要负责控制电调输出的PWM信号,以控制电机转速。以下是一个简单的PWM控制代码示例: ```c++ #include <px4_platform_common/px4_config.h> #include <px4_platform_common/tasks.h> #include <drivers/drv_pwm_output.h> int main(int argc, char *argv[]) { // 初始化PWM输出 px4_arch_configgpio(GPIO_PWM_OUT1); px4_arch_configgpio(GPIO_PWM_OUT2); px4_arch_configgpio(GPIO_PWM_OUT3); px4_arch_configgpio(GPIO_PWM_OUT4); // 配置PWM输出通道参数 struct pwm_output_values pwm_values; pwm_values.channel_count = 4; pwm_values.default_value = 1000; pwm_values.values[0] = 1000; pwm_values.values[1] = 1000; pwm_values.values[2] = 1000; pwm_values.values[3] = 1000; ioctl(PWM_OUTPUT_GET_CHANNEL_COUNT, 0, (unsigned long)&pwm_values); // 控制PWM输出 while (true) { pwm_values.values[0] = 1200; pwm_values.values[1] = 1300; pwm_values.values[2] = 1400; pwm_values.values[3] = 1500; ioctl(PWM_OUTPUT_SET_SAFETY_LOCK, 0); ioctl(PWM_OUTPUT_SET_ARM_OK, 0); ioctl(PWM_OUTPUT_SET_DISARMED_PWM, 0, pwm_values.default_value); ioctl(PWM_OUTPUT_SET_MODE, PWM_OUTPUT_MODE_ONESHOT); ioctl(PWM_OUTPUT_SET_CONFIG, 0, (unsigned long)&pwm_values); ioctl(PWM_OUTPUT_UPDATE_CHANNELS, 0, (unsigned long)&pwm_values); usleep(20000); } return 0; } ``` 在上述示例代码中,首先通过调用`px4_arch_configgpio`函数初始化PWM输出引脚,然后配置PWM输出通道参数。在控制PWM输出时,通过调用`ioctl`函数设置PWM输出的安全锁、启动状态、解锁时的PWM值、输出模式和输出参数,最后通过调用`ioctl`函数更新PWM输出值。 需要注意的是,上述示例代码中的PWM输出值仅作为示例,实际应用中需要根据具体需求进行调整。同时,为了保证PWM输出的稳定性和安全性,通常需要在飞控系统中实现一些保护机制,如输出PWM频率限制、输出值范围限制等。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值