//主进程
//初始状态为MANUAL_CTRL
/*
Finite State Machine
控制系统启动
|
|
v
----- > 手动控制 <---------------
| ^ | \ |
| | | \ |
| | | >自动起飞 |
| | | / |
| | | / |
| | | / |
| | v / |
| 悬停模式 < |
| ^ | \ \ |
| | | \ \ |
| | | > 自动降落 ---
| | |
| | v
-------- 程序控制(大概?)
*/
void PX4CtrlFSM::process()
{
ros::Time now_time = ros::Time::now();
Controller_Output_t u;
Desired_State_t des(odom_data);
bool rotor_low_speed_during_land = false;
// STEP1: state machine runs
switch (state)
{
case MANUAL_CTRL:
{
if (rc_data.enter_hover_mode) // Try to jump to AUTO_HOVER
//enter_hover_mode的布尔值由遥控器通道4值决定，last_mode小于1750本次大于1750则置为True
//表示将模式切换至AUTO_HOVER
{
if (!odom_is_received(now_time))
//如果里程计数据更新时间超过设定值，则视为里程计丢失，参数中为0.5S
{
ROS_ERROR("[px4ctrl] Reject AUTO_HOVER(L2). No odom!");
break;
}
//如果上次控制指令更新时间*没有超过*设定值，则禁止进入AUTO_HOVER模式
if (cmd_is_received(now_time))
{
ROS_ERROR("[px4ctrl] Reject AUTO_HOVER(L2). You are sending commands before toggling into AUTO_HOVER, which is not allowed. Stop sending commands now!");
break;
}
if (odom_data.v.norm() > 3.0)
//个人理解为里程计速度向量长度若大于3，则认为数据错误
{
ROS_ERROR("[px4ctrl] Reject AUTO_HOVER(L2). Odom_Vel=%fm/s, which seems that the locolization module goes wrong!", odom_data.v.norm());
break;
}
state = AUTO_HOVER;
//更新状态
controller.resetThrustMapping();
//函数如下：param_.gra为重力加速度，param_.thr_map.hover_percentage在参数表为0.30
//Thrust percentage in Stabilize/Arco mode
//LinearControl::resetThrustMapping(void)
//{
//thr2acc_ = param_.gra / param_.thr_map.hover_percentage;
//P_ = 1e6;
//}
set_hov_with_odom();
//将里程计数据写入一个四位列向量
toggle_offboard_mode(true);
//切换到offboard模式,下面对应函数解析
ROS_INFO("\033[32m[px4ctrl] MANUAL_CTRL(L1) --> AUTO_HOVER(L2)\033[32m");
}
else if (param.takeoff_land.enable && takeoff_land_data.triggered && takeoff_land_data.takeoff_land_cmd == quadrotor_msgs::TakeoffLand::TAKEOFF) // Try to jump to AUTO_TAKEOFF
{
//切换到自动起飞的条件
//1.参数表中自动起飞参数为True
//2.自动起飞标志位置为True->在Takeoff_Land_Data_t::feed中(服务takeoff_land_sub的回调函数)
//3.读取mavros/takeoff_land_cmd的状态为已解锁
if (!odom_is_received(now_time))
{
ROS_ERROR("[px4ctrl] Reject AUTO_TAKEOFF. No odom!");
break;
}
if (cmd_is_received(now_time))
{
ROS_ERROR("[px4ctrl] Reject AUTO_TAKEOFF. You are sending commands before toggling into AUTO_TAKEOFF, which is not allowed. Stop sending commands now!");
break;
}
if (odom_data.v.norm() > 0.1)
//认为自动起飞时应该是静态的
{
ROS_ERROR("[px4ctrl] Reject AUTO_TAKEOFF. Odom_Vel=%fm/s, non-static takeoff is not allowed!", odom_data.v.norm());
break;
}
if (!get_landed())
//无人机应处于着陆状态
{
ROS_ERROR("[px4ctrl] Reject AUTO_TAKEOFF. land detector says that the drone is not landed now!");
break;
}
if (rc_is_received(now_time)) // Check this only if RC is connected.
//如果使用遥控器(非必须),下面应该是通道值的检测,不深入.
{
if (!rc_data.is_hover_mode || !rc_data.is_command_mode || !rc_data.check_centered())
{
ROS_ERROR("[px4ctrl] Reject AUTO_TAKEOFF. If you have your RC connected, keep its switches at \"auto hover\" and \"command control\" states, and all sticks at the center, then takeoff again.");
while (ros::ok())
{
ros::Duration(0.01).sleep();
ros::spinOnce();
if (rc_data.is_hover_mode && rc_data.is_command_mode && rc_data.check_centered())
{
ROS_INFO("\033[32m[px4ctrl] OK, you can takeoff again.\033[32m");
break;
}
}
break;
}
}
state = AUTO_TAKEOFF;
//更新状态
controller.resetThrustMapping();
//上面有描述
set_start_pose_for_takeoff_land(odom_data);
//记录起飞位置和起飞时间
toggle_offboard_mode(true); // toggle on offboard before arm
//切换offboard模式,下面有详解
for (int i = 0; i < 10 && ros::ok(); ++i) // wait for 0.1 seconds to allow mode change by FMU // mark
{
ros::Duration(0.01).sleep();
ros::spinOnce();
}
//延时0.1秒再次检测模式,防误触?
if (param.takeoff_land.enable_auto_arm)
{
toggle_arm_disarm(true);
}
takeoff_land.toggle_takeoff_land_time = now_time;
//mark自动起飞时间
ROS_INFO("\033[32m[px4ctrl] MANUAL_CTRL(L1) --> AUTO_TAKEOFF\033[32m");
}
if (rc_data.toggle_reboot) // Try to reboot. EKF2 based PX4 FCU requires reboot when its state estimator goes wrong.
{
if (state_data.current_state.armed)
{
ROS_ERROR("[px4ctrl] Reject reboot! Disarm the drone first!");
break;
}
reboot_FCU();
}
//没仔细看,应该是异常检测rc_data.toggle_reboot在读取RC服务是通过判断置为True
break;
}
case AUTO_HOVER:
{
if (!rc_data.is_hover_mode || !odom_is_received(now_time))
//如果不是在悬停模式下,或里程计数据更新未超时(这个判断有点没明白什么意思)
{
state = MANUAL_CTRL;
//更新状态为手动控制
toggle_offboard_mode(false);
//退出offboard模式
ROS_WARN("[px4ctrl] AUTO_HOVER(L2) --> MANUAL_CTRL(L1)");
}
else if (rc_data.is_command_mode && cmd_is_received(now_time))
{
//如果在程序控制模式下,且指令更新未超时
if (state_data.current_state.mode == "OFFBOARD")
{
//如果在offboard模式下
state = CMD_CTRL;
//更新状态为程序控制模式
des = get_cmd_des();
//更新控制数据 从话题cmd的回调函数中得到
ROS_INFO("\033[32m[px4ctrl] AUTO_HOVER(L2) --> CMD_CTRL(L3)\033[32m");
}
}
else if (takeoff_land_data.triggered && takeoff_land_data.takeoff_land_cmd == quadrotor_msgs::TakeoffLand::LAND)
{
//不分析了,如果这样就自动降落
state = AUTO_LAND;
set_start_pose_for_takeoff_land(odom_data);
ROS_INFO("\033[32m[px4ctrl] AUTO_HOVER(L2) --> AUTO_LAND\033[32m");
}
else
{
set_hov_with_rc();
//从RC数据中获取位置期望,具体下面有解析
des = get_hover_des();
//将获取的位置作为输入传给DES
if ((rc_data.enter_command_mode) ||
(takeoff_land.delay_trigger.first && now_time > takeoff_land.delay_trigger.second))
{
takeoff_land.delay_trigger.first = false;
//第一次进入该模式发送提示
publish_trigger(odom_data.msg);
ROS_INFO("\033[32m[px4ctrl] TRIGGER sent, allow user command.\033[32m");
}
// cout << "des.p=" << des.p.transpose() << endl;
}
break;
}
case CMD_CTRL:
{
//程序控制模式
if (!rc_data.is_hover_mode || !odom_is_received(now_time))
{
state = MANUAL_CTRL;
//更新状态为手动模式
toggle_offboard_mode(false);
//关闭offboard模式
ROS_WARN("[px4ctrl] From CMD_CTRL(L3) to MANUAL_CTRL(L1)!");
}
else if (!rc_data.is_command_mode || !cmd_is_received(now_time))
//如果不在控制模式下,且控制数据更新超时
{
state = AUTO_HOVER;
//更新状态为定点模式
set_hov_with_odom();
//保存当前位置
des = get_hover_des();
//将位置数据传至DES
ROS_INFO("[px4ctrl] From CMD_CTRL(L3) to AUTO_HOVER(L2)!");
}
else
{
des = get_cmd_des();
//将控制数据传至DES
}
if (takeoff_land_data.triggered && takeoff_land_data.takeoff_land_cmd == quadrotor_msgs::TakeoffLand::LAND)
{
//如果在程序控制模式下降落,需先切换回定点模式
ROS_ERROR("[px4ctrl] Reject AUTO_LAND, which must be triggered in AUTO_HOVER. \
Stop sending control commands for longer than %fs to let px4ctrl return to AUTO_HOVER first.",
param.msg_timeout.cmd);
}
break;
}
case AUTO_TAKEOFF:
{
//自动起飞
if ((now_time - takeoff_land.toggle_takeoff_land_time).toSec() < AutoTakeoffLand_t::MOTORS_SPEEDUP_TIME) // Wait for several seconds to warn prople.
{
des = get_rotor_speed_up_des(now_time);
//切换到自动起飞后,怠速时间为3S,期间更新DES
}
else if (odom_data.p(2) >= (takeoff_land.start_pose(2) + param.takeoff_land.height)) // reach the desired height
{
//到达设定高度后设置为定点模式
state = AUTO_HOVER;
set_hov_with_odom();
ROS_INFO("\033[32m[px4ctrl] AUTO_TAKEOFF --> AUTO_HOVER(L2)\033[32m");
takeoff_land.delay_trigger.first = true;
takeoff_land.delay_trigger.second = now_time + ros::Duration(AutoTakeoffLand_t::DELAY_TRIGGER_TIME);
}
else
{
//正在上升
des = get_takeoff_land_des(param.takeoff_land.speed);
}
break;
}
case AUTO_LAND:
{
//自动降落
if (!rc_data.is_hover_mode || !odom_is_received(now_time))
{
//未在定点模式或里程计未及时更新
state = MANUAL_CTRL;
//切换回手动模式
toggle_offboard_mode(false);
//退出offboard
ROS_WARN("[px4ctrl] From AUTO_LAND to MANUAL_CTRL(L1)!");
}
else if (!rc_data.is_command_mode)
{
//若不在控制模式下
state = AUTO_HOVER;
//切换回定点模式
set_hov_with_odom();
des = get_hover_des();
ROS_INFO("[px4ctrl] From AUTO_LAND to AUTO_HOVER(L2)!");
}
else if (!get_landed())
//
{
des = get_takeoff_land_des(-param.takeoff_land.speed);
}
else
{
rotor_low_speed_during_land = true;
//怠速
static bool print_once_flag = true;
if (print_once_flag)
{
ROS_INFO("\033[32m[px4ctrl] Wait for abount 10s to let the drone arm.\033[32m");
print_once_flag = false;
}
//灯带十秒上锁
if (extended_state_data.current_extended_state.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND) // PX4 allows disarm after this
{
static double last_trial_time = 0; // Avoid too frequent calls
if (now_time.toSec() - last_trial_time > 1.0)
{
if (toggle_arm_disarm(false)) // disarm
{
print_once_flag = true;
state = MANUAL_CTRL;
//上锁后切回手动模式
toggle_offboard_mode(false); // toggle off offboard after disarm
ROS_INFO("\033[32m[px4ctrl] AUTO_LAND --> MANUAL_CTRL(L1)\033[32m");
}
last_trial_time = now_time.toSec();
}
}
}
break;
}
default:
break;
}
// STEP2: estimate thrust model
//选择合适的计算方式
if (state == AUTO_HOVER || state == CMD_CTRL)
{
//若处于程控或定点模式下
// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);
controller.estimateThrustModel(imu_data.a,param);
//将imu加速度和参数输入estimateThrustModel
}
// STEP3: solve and update new control commands
if (rotor_low_speed_during_land) // used at the start of auto takeoff
{
motors_idling(imu_data, u);
//将推力计算入模型->起飞时使用,这也解释了为什么起飞时会飞机会飘,起飞时不使用视觉里程计
}
else
{
debug_msg = controller.calculateControl(des, odom_data, imu_data, u);
//将推力，des，里程计数据，imu数据输入
debug_msg.header.stamp = now_time;
debug_pub.publish(debug_msg);
}
// STEP4: publish control commands to mavros
//将结果发布给mavros
if (param.use_bodyrate_ctrl)
{
publish_bodyrate_ctrl(u, now_time);
}
else
{
publish_attitude_ctrl(u, now_time);
}
// STEP5: Detect if the drone has landed
land_detector(state, des, odom_data);
// cout << takeoff_land.landed << " ";
// fflush(stdout);
// STEP6: Clear flags beyound their lifetime
//清理标志位
rc_data.enter_hover_mode = false;
rc_data.enter_command_mode = false;
rc_data.toggle_reboot = false;
takeoff_land_data.triggered = false;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
