高飞老师的PX4Ctrl整体状态机代码整体框架

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  
	//且给出每个状态的期望位姿 des 。用于后续控制	
    switch (state){
    }

    // STEP2: estimate thrust model
	if (state == AUTO_HOVER || state == CMD_CTRL)
	{
		// 用来计算thr2acc_ 和 P_ ，后续用于给 calculateControl（）函数 由加速度来计算推力
		controller.estimateThrustModel(imu_data.a,param);
	}

	// 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);
		//用于debug
		debug_msg.header.stamp = now_time;
		debug_pub.publish(debug_msg);
	}

	// STEP4: publish control commands to 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;
}