pos_control模块主要实现飞行器的位置控制(也包括速度控制)。即根据设定的目标位置/速度,给出需要实现的飞行器姿态,从而使飞行器保持在设定的目标位置或速度。位置控制的输出为飞行器的姿态,为下一级姿态控制(att_control)的输入值。
以多旋翼为例,pos_control在对应的模块目录mc_pos_control下实现,其主要运行函数为mc_pos_control_main.cpp中的MulticopterPositionControl::task_main()函数
在这里插入图片描述
函数运行首先做一些包括消息订阅等的初始化,然后进入while循环,同其它模块一样,while循环按照一定频率运行,在每个循环中执行位置控制的功能。
在这里插入图片描述
1 获取控制需要的数据,包括飞行器状态(vehicle_status),降落状态(land_detect),飞行器姿态(attitude),控制模式(control_mode),位置(local_pos),位置设定值(pos_sp_triplet)等
2 更新参数
3 重置标志位(降落情况下)
4 更新设定值(_ref_alt,_pos_sp)
5 更新速度的导数(update velocity derivative)
6 设置飞行任务(flight task set),flight task包括高度控制,位置控制,手动控制,None。
7 执行控制任务(do_control),分为两种模式,带手动控制,和不带手动控制


  • 带手动控制,运行control_manual()函数,control_manual()程序步骤如下
    1 首先计算手动控制速度设定值(man_vel_sp)
    2 根据控制标志位所示不同情况,计算速度设定值(_vel_sp),以及设定需要执行的控制功能——高度控制(_run_alt_control)或者位置控制(_run_pos_control)
    3 执行位置控制(control_position),在位置控制中,执行
    3.1 在_run_alt_control或_run_pos_control使能的情况下,通过高度/位置(_pos_sp)计算速度设定值(_vel_sp),如果没有设定这两个标志位,则不通过高度和位置设定值来计算_vel_sp,直接采用给定的速度设定值。
    3.2 计算需要推力设定值(thrust_sp),根据速度和速度设定值的误差,以及推力设定值(thrust_sp)计算姿态设定值(_att_sp)

  • 不带手动控制,运行control_non_manual函数,程序步骤如下
    1 运行control_auto函数,根据跟踪类型从_pos_sp_triplet计算_pos_sp或者_vel_sp,以及设定控制标志位
    2 运行control_position,control_position的作用同前面一样


总体而言,pos_control的控制功能就是根据control_mode里面标志位来设定_pos_sp,_vel_sp,以及_run_alt_control,_run_pos_sp标志位,主要功能由control_position实现,它的控制作用就是根据_pos_sp或者_vel_sp实现位置控制或速度控制。


control_position输出的姿态设定值(_att_sp)会传送给后面的att_control进行姿态控制。


在这里插入图片描述
8 发布位置设定值(_local_pos_sp),包括位置设定值(_pos_sp)、速度设定值(_vel_sp)、加速度设定值(_acc_sp)等信息
9 产生姿态设定值(_att_sp)
10 发布姿态设定值