EGO_Planner代码学习(三):px4ctrl代码流程
上节阅读了EGO_Planner的轨迹服务器部分的代码,其实质是将规划器生成的轨迹进行离散化,间隔10ms计算当前时刻在轨迹上的位置,并发送到
/position_cmd
话题上。
这两天抽空看了下控制器部分的代码,在
realflight_moudles\px4ctr
l这个软件包里,它的功能是进行px4的模式识别和上层控制,通过线性控制器把
/position_cmd
发布过来的位置命令解算成飞控的控制量,通过mavros发送给px4。下面就详细看一下它的启动过程
px4ctrl代码流程
px4ctrl软件包下一共有5个C++文件,它们的作用分别是:
px4ctrl_node.cpp //控制器主节点
PX4CtrlFSM.cpp //状态机,负责模式切换
controller.cpp //控制器,根据期望位姿计算控制量
input.cpp //处理订阅的相关ros数据,方便后面使用
PX4CtrlParam.cpp //参数服务器,从ros参数服务器中载入用户参数
这个功能包的入口在px4ctrl_node.cpp
,其代码流程如下:
1-> 初始化结点
2-> 检测终止信号
3-> 初始化参数类,从参数服务器读取参数
4-> 初始化线性控制器
5-> 初始化状态机
6-> 订阅当前状态 ,以参数传给fsm.state_data.feed() //接收状态消息
7-> 订阅当前扩展状态 ,以参数传给fsm.extened_state_data.feed() //接收扩展状态消息
8-> 订阅里程计话题odmo ,由/vins_fusion/imu_propagate映射而来
回调函数 ->读取位置、速度、四元数数据传入fsm.odmo_data,监控数据帧率,小于100hz警告
9-> 订阅位置命令话题cmd ,由/position_cmd映射而来
回调函数 ->读取位置、速度、加速度、加加速度、yaw角给fsm.cmd_data
10->订阅imu数据话题 ,/mavros/imu/data
回调函数 ->读取姿态、角速度、角加速度给fsm.imu_data
----------------------------------------- 接收机通道定义------------------------------------------------------
更新:2023.6.6 15.23
接收机通道定义
ch[0]: 横滚
ch[1]: 俯仰
ch[2]: 油门
ch[3]: 偏航
开关 //打到最上面为0,打到最下面为1.0
两档:0~1.0
三档:0~0.5~1.0
ch[4]: 5通道,飞行模式,三档
ch[5]: 6通道,命令控制,两档
ch[7]: 8通道,用于重启px4
---------------------------------------------------------------------------------------------------------------
12-> 订阅接收机数据话题 ,/mavros/rc/in
回调函数 ->读取四个通道的打杆值,并进行模式识别
//通道5
-> 是否上次的挡位是在上面,当前挡位是在下面
是: enter_hover_mode = 1
否: enter_hover_mode = 0
-> 是否当前挡位是在下面
是: is_hover_mode = 1
否: is_hover_mode = 0
-> 如果is_hover_mode == 1
//通道6
-> 是否上次的挡位是在上面,当前挡位是在下面
是: enter_command_mode = 1
否: enter_command_mode = 0
-> 是否当前挡位是在下面
是: is_command_mode = 1
否: is_command_mode = 0
-> 如果is_hover_mode != 1 且 is_command_mode != 1 //5、6通道都没打到位
是: 若8通道打到内侧:toggle_reboot=1;否则toggle_reboot=0
否: toggle_reboot=0 //正常情况下不给重启px4
13-> 订阅电池状态话题 ,/mavros/battery
回调函数 ->读取电压、计算百分比并打印
14-> 订阅起飞命令话题,takeoff_land
回调函数 ->fsm.takeoff_land_data.triggered = 1,接收起飞命令
15-> 创建发布者 fsm.ctrl_FCU_pub 到话题 /mavros/setpoint_raw/attitude
16-> 创建发布者 fsm.traj_start_trigger_pub 到话题 /traj_start_trigger
17-> 创建发布者 fsm.debug_pub 到话题 /debugPx4ctrl
18-> 创建服务器 fsm.set_FCU_mode_srv 到话题 /mavros/set_mode
19-> 创建服务器 fsm.arming_cline_srv 到话题 /mavros/cmd/arming
20-> 创建服务器 fsm.reboot_FCU_srv 到话题 /mavros/cmd/command
21-> 判断从参数服务器读过来的参数设置是否需要遥控:1不需要,0需要
-> 不需要,警告:遥控失效,小心
-> 需要:等待接收机信号
22-> 进入死循环,检查px4的连接,连接正常跳出循环
23-> 主循环,以固定帧率进入fsm.process()进程
-> 记录时间戳 now_time,定义控制器输出u,期望状态des
//u就是直接给px4的控制量,des是期望的位姿
-> 状态识别 //状态机的灵魂,值得借鉴!!!
这里将控制大致分为了三级:
L1:手动控制
L2:自动悬停
L3:命令控制(跟踪轨迹)
介于L1和L2之间的:自动起飞\降落,L3模式不允许自动降落
//整个状态机在做的就是根据模式选择相应的期望位姿des,后面会传给控制器
-> 手动模式(MANUAL_CTRL):
-> if尝试进入自动悬停模式 //5通道刚刚打下来了
-> 如果没里程计信息,报错,跳出switch
-> 如果收到控制指令,报错,跳出switch
-> 如果当前速度太快,报错,跳出switch
-> 更新状态为AUTO_HOVER
-> 推力映射
-> 设置悬停位姿的位置、方向
-> 设置px4进入offboard模式//只要是L2以上的控制,px4都运行在offboard模式
//在L1收到了起飞命令的触发
-> else if使能自动起飞,且起飞命令被触发,且起飞命令等于1
-> 如果没里程计信息,报错,跳出switch
-> 如果收到控制指令,报错,跳出switch
-> 如果当前速度太快,报错,跳出switch
-> 如果不在陆地模式,报错,跳出switch
-> 若当前能收到遥控器信号
//自动起飞要求
-> 如果5通不在下侧,6通不在下侧,ch[0]<1e-5(?,不理解第三个条件)
死循环等待所有条件满足,跳出switch//下次进来再执行后面的代码吧
//满足了自动起飞要求
-> 更新状态为AUTO_HOVER
-> 推力映射//推力模型部分还是不太懂,写完查一查
-> 设置自动起飞起始位姿的位置、方向
-> 设置px4进入offboard模式
-> 如果使能了auto_arm,则px4进入ARM模式(解锁px4)
-> 记录切换到自动起飞的时间戳
-> 如果想要重启px4
-> 检查px4状态是否为armed,是则报错,跳出switch
-> 重启px4
-> 跳出switch
-> 自动悬停(AUTO_HOVER):
//L2的第一条件就是5通在最下面,把5通拨上去直接退到L1
-> if 5通不在最下面,或没有收到里程计信息
-> 更新状态为MANUAL_CTRL
-> 设置px4退出offboard模式
//L2->L3的唯一途径,6通已经打到最下面
-> else if当前6通在最下面,且正在接收命令
-> 如果px4当前状态为offboard
-> 更新状态为CMD_CTRL
//把/position_cmd发来的数据作为des
-> 从控制命令读取期望的位置、速度、加速度、加加速度、yaw角、角加速度
-> else if自动起飞命令被触发,且起飞命令=0
-> 更新状态为AUTO_LAND
-> 设置降落的起始位姿
//需要切换状态的条件都不满足,就停留在L2,用遥控器输入做为控制量
-> else
-> 从遥控器接收期待控制量,设置为期望位姿
-> 如果6通打了下来,或者使能自动起飞\降落延迟触发
-> fsm.traj_start_trigger_pub发送里程计消息(不理解这个消息,给规划器用的吗?,告诉规划器可以发命令过来?)
-> 跳出switch
-> 命令控制(CMD_CTRL):
//L2的前提已经要求5通在下了,L3更要求了
-> if 5通不在最下面,或没有收到里程计信息
-> 更新状态为MANUAL_CTRL
-> 设置px4退出offboard模式
//从L3->L2
-> else if 6通不在最下面,或没有收到控制命令
-> 更新状态为AUTO_HOVER
-> 设置px4退出offboard模式
-> 设置从里程计获得位姿作为悬停位姿
-> 设置悬停位姿为期望位姿
//需要切换状态的条件都完了,其实就是5、6通拨下来,且/position_cmd不断地发控制命令
-> else 从控制命令获得期望位姿
-> if 这时自动起飞\降落命令被触发,且要自动降落
-> 报错,不能在命令控制模式触发自动降落,只能在自动悬停下启动
-> 跳出switch
-> 自动起飞(AUTO_TAKEOFF):
-> 刚收到自动起飞命令的前几秒怠速
-> if起飞高度大于设置的悬停高度
//已经达到了起飞高度
-> 更新状态为AUTO_HOVER
-> 从里程计获得当前位姿作为悬停位姿
-> takeoff_land.delay_trigger.first = true
//使能并设置触发延迟(?,可能是进入自动悬停模式一段时间后告诉规划器我ok了)
//这段延迟时间被宏定义为2S
-> takeoff_land.delay_trigger.second = now_time + ros::Duration(AutoTakeoffLand_t::DELAY_TRIGGER_TIME)
-> else计算期望位姿des
-> 跳出switch
-> 自动降落(AUTO_LAND):
//能自动降落必在L2,看看符不符合L2的条件
-> if 5通不在最下面,或没有收到里程计信息
-> 更新状态为MANUAL_CTRL
-> 设置px4退出offboard模式
//这个条件感觉多余了
-> else if 6通不在最下面
-> 更新状态为AUTO_HOVER
-> 从里程计获得当前位姿作为悬停位姿
-> else if还没降落
-> 计算期望位姿
//已经降落了
-> else
-> rotor_low_speed_during_land = 1(?不懂,看完代码感觉像怠速标志位)
-> 如果px4扩展状态为已经着陆//px4真牛皮
-> 给px4上锁,状态更新为MANUAL_CTRL,px4退出offboard模式
-> 跳出switch
-> 默认:
-> 跳出switch
//状态机的代码走完了,最终结果就是计算des,下面开始计算控制量u
-> if:状态为AUTO_HOVER或CMD_CTRL
//要飞的模式,这个推力模型和前面那个推力映射不太明白
-> 计算推力模型
//算u
-> 计算控制量
-> if电机怠速,计算对应的u
-> else
-> 计算对应的u
-> 发布调试信息debug_msg
-> 通过mavros,发布控制命令u//根据用户参数判断是控制量是姿态还是角速度,发布到相应的话题上
-> 检测着陆状态//还挺麻烦
-> if L2,L3模式下,false//那肯定不能在陆地上
-> if L1且px4上锁,true
-> if 速度,位置都小于阈值且有一会了,true
-> 重置标志位
梳理完整个控制脉络清晰多了,其实这里稍微省略了一下控制器部分根据des解算u的代码,具体可以查看controller.cpp部分,源码使用的线性控制器,感觉整体响应比较慢,可能是我给的速度加速度阈值比较小,尝试换成非线性控制器可能会好一点。规划器部分的代码也在阅读,只是整体结构比较庞大,还在慢慢梳理。