EGO_Planner代码学习（三）：px4ctrl代码流程

上节阅读了EGO_Planner的轨迹服务器部分的代码，其实质是将规划器生成的轨迹进行离散化，间隔10ms计算当前时刻在轨迹上的位置，并发送到 /position_cmd话题上。
这两天抽空看了下控制器部分的代码，在 realflight_moudles\px4ctrl这个软件包里，它的功能是进行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部分，源码使用的线性控制器，感觉整体响应比较慢，可能是我给的速度加速度阈值比较小，尝试换成非线性控制器可能会好一点。规划器部分的代码也在阅读，只是整体结构比较庞大，还在慢慢梳理。