一、参数定义
1.1 从ROS参数服务器中获取的
- uav_id
- sim_mode
- 注：下面参数通过 load_communication_param() 函数加载
- pos_controller：控制器选择，默认为PX4，来自yaml文件
- enable_external_control：使用外部控制器，默认为false，来自yaml文件
- Takeoff_height：起飞高度，来自yaml文件
- Disarm_height：降落时，自动上锁高度，来自yaml文件
- Land_speed：降落速度，来自yaml文件
- 地理围栏：来自yaml文件
- control_state：初始化为INIT
- uav_control_state.failsafe：安全检查，默认为false
- Agent_CMD：初始化为INIT_Pos_Hover，用于无人机移动到指定起飞位置
- position_ref ：期望位置
- yaw_ref ：期望偏航角
- pos_des：期望位置？
- vel_des：期望速度？
- acc_des：期望加速度？
- quick_land：初始化为false
- set_landing_des：初始化为 false，是否需要设置降落位置和速度
- offset_pose.x，offset_pose.y：无人机位置偏移量，初始化为0。来自OffsetPose.msg
- uav_pose：无人机位置，初始化为0。来自uav_state_sub 订阅器
- uav_vel：无人机速度，初始化为0。来自uav_state_sub 订阅器
- u_att：滚转角、俯仰角、偏航角、推力，初始化为0
- rc_input.init()：uav_controller.h文件中创建名为 `
rc_input` 的 `RC_Input` 类的对象；`RC_Input` 类在rc_input.h文件中。链接：
1.2 订阅消息得到的参数
- 创建 uav_cmd_sub 订阅器，使用 uav_cmd_cb() 函数处理订阅到的消息
- 创建 uav_state_sub 订阅器，使用 uav_state_cb() 函数处理订阅到的消息
/* msg 包含的参数见UAVState.msg float32[3] attitude：欧拉角 attitude_q：四元数 -------> 欧拉角转四元数 得到uav_pos、uav_vel、uav_yaw、Takeoff_position */
1.3 发布的参数
- 创建 uav_control_state_pub 发布器，接收 uav_control_state，来自mainloop() 函数（2）检查无人机控制状态，据此执行不同的程序
- 创建 px4_setpoint_raw_local_pub 发布器，接收pos_setpoint，NEU坐标系下的位置、速度、加速度期望值。飞控有对应接收器
1.4 服务通信参数
- 创建 px4_set_mode_client 客户端，使用 set_px4_mode_func()函数接收mode，通过客户端 call() 函数向服务端发送请求，将设置的mode发送到飞控
二、成员函数
2.1 mainloop()
（1）安全检查
- safety_flag：check_failsafe() 函数的返回值
- uav_state.odom_valid 的值根据 UAV_estimator::check_uav_state() 得到
- safety_flag == -1：与PX4断开连接，直接返回
- safety_flag == 1 ：超出geofence（地理围栏），failsafe=true，control_state=降落
- safety_flag == 2 ：检测到odom（定位数据）失效，quick_land=true，failsafe=true，control_state=降落
- safety_flag == 3 ：与遥控器断连，failsafe=true，control_state=降落
- safety_flag == 0 ：通过安全检查，failsafe=false
- 若uav_state 在 RC_POS_CONTROL 或 COMMAND_CONTROL 模式下，飞控需为OFFBOARD模式
（2）检查无人机控制状态，据此执行不同的程序
- INIT 状态：飞控切换至 POSCTL 模式
- RC_POS_CONTROL 状态：调用 set_hover_pose_with_rc () 函数，得到悬停位置
// 悬停位置计算不是很明白，Hover_position与rc_input.ch如何对应- COMMAND_CONTROL 状态：
- 若为PX4 控制模式：调用 set_command_des() 函数
/* Agent_CMD = Init_Pos_Hover: home点上方悬停 得到pos_des、vel_des、acc_des、yaw_des；Takeoff_position在哪赋值？ Agent_CMD = Current_Pos_Hover：当前位置悬停 若上一条命令不是在当前位置悬停，则悬停位置=无人机位置；悬停角度=无人机角度 否则，无人机已经在上条命令悬停，此次位置不变，期望位置=悬停位置；航向角=悬停角度 Agent_CMD = Land： 切换control_state = LAND_CONTROL Agent_CMD = Move Move_mode = XYZ_POS 惯性系定点 得到pos_des、yaw_des Move_mode = XY_VEL_Z_POS 惯性系定高速度控制 发布指令要预先定义target_pos，target_vel；得到pos_des、yaw_des、vel_des Move_mode = XYZ_VEL 惯性系速度控制 发布指令要预先定义target_vel；得到yaw_des、vel_des Move_mode = XYZ_POS_BODY 机体系定点 发布指令要预先定义target_pos(机体系下)；调用rotation_yaw函数，把它转为惯性系；得到 pos_des、yaw_des Move_mode = XYZ_VEL_BODY 机体系速度控制 发布指令要预先定义target_vel(机体系下)；调用rotation_yaw函数，把它转为惯性系；得到 vel_des、yaw_des、yaw_rate_des Move_mode = XY_VEL_Z_POS_BODY 机体系定高速度控制 发布指令要预先定义target_pos，target_vel；调用rotation_yaw函数，把target_vel转为惯性系 得到pos_des、yaw_des、vel_des */- 若为PID、UDE、NE：调用 set_command_des_for_pos_controller() 函数
- LAND_CONTROL 状态：
- 若定位来源为GPS或RTK：飞控切换为AUTO.LAND模式
- 否则：
- set_landing_des=false时：
- Land_speed=1.0，用于 safety_flag == 2 的情况
- 在当前位置降落，令set_landing_des=true
- 无人机 z 坐标低于指定高度，进入急停，调用 enable_emergency_func()
- uav_state.armed=false：无人机上锁，降落结束
- control_state=INIT，Agent_CMD=INIT_POS_Hover
- 使用 uav_control_state_pub 发布器，发布状态后，如果是INIT，直接返回
- 把上述不同uav控制状态得到的位置控制指令发送至pos_controller
- 若为PX4，调用 send_pos_cmd_to_px4_original_controller() 函数
- control_state=RC_POS_CONTROL：调用 send_pos_setpoint() 函数
/* 发送期望位置至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：设置位掩码，指示哪些维度信息需要忽略。问：IGNORE_AFY重复出现？ FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：位置信息为x、y、z、yaw 使用 px4_setpoint_raw_local_pub 发布器 */- control_state=LAND_CONTROL 且飞控mode=OFFBOARD
- 若速降：调用send_vel_setpoint() 函数，用于 safety_flag == 2 的情况
/* 发送期望位置至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：速度信息为vx、vy、vz、yaw 使用 px4_setpoint_raw_local_pub 发布器 */- 否则，调用send_pos_vel_xyz_setpoint() 函数
/* 发送期望位置至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：速度信息为x、y、z、vx、vy、vz、yaw 使用 px4_setpoint_raw_local_pub 发布器，飞控接收 */- control_state=COMMAND_CONTROL
- 若Agent_CMD = Init_Pos_Hover 或 Agent_CMD = Current_Pos_Hover：调用 send_pos_setpoint() 函数
- 若Agent_CMD = Move
- 若Move_mode为XYZ_POS或XYZ_POS_BODY：调用send_pos_setpoint() 函数
- 若Move_mode = XY_VEL_Z_POS或XY_VEL_Z_POS_BODY：
- Yaw_Rate_Mode为True：偏航角速率，调用send_vel_xy_pos_z_setpoint_yaw_rate()
/* 发送期望位置和速度至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：位置和速度信息为z、vx、vy、0、yaw_rate 使用 px4_setpoint_raw_local_pub 发布器，飞控接收 */- Yaw_Rate_Mode为false：调用send_vel_xy_pos_z_setpoint()
/* 发送期望位置和速度至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：位置和速度信息为z、vx、vy、0、yaw 使用 px4_setpoint_raw_local_pub 发布器，飞控接收 */- 若Move_mode = XYZ_VEL或XYZ_VEL_BODY：
- Yaw_Rate_Mode为True：偏航角速率，调用send_vel_setpoint_yaw_rate()
/* 发送速度至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：位置和速度信息为vx、vy、vz、yaw_rate 使用 px4_setpoint_raw_local_pub 发布器，飞控接收 */- Yaw_Rate_Mode为false：调用send_vel_setpoint()
/* 发送期望速度至飞控 pos_setpoint：存储位置设定点信息 pos_setpoint.type_mask：0b代表二进制，1代表ignore FRAME_LOCAL_NED：本地东北天坐标系 pos_setpoint：位置和速度信息为vx、vy、vz、yaw 使用 px4_setpoint_raw_local_pub 发布器，飞控接收 */- 否则，调用
182
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
