void
MulticopterPositionControl::task_main()
{

	_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);

	/*
	 * do subscriptions
	 */
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));


	parameters_update(true);

	/* initialize values of critical structs until first regular update */
	_arming.armed = false;

	/* get an initial update for all sensor and status data */
	poll_subscriptions();

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool reset_yaw_sp = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;

	math::Vector<3> thrust_int;
	thrust_int.zero();


	math::Matrix<3, 3> R;
	R.identity();

	/* wakeup source */
	px4_pollfd_struct_t fds[1];

	fds[0].fd = _local_pos_sub;//主要是判断是否有当地位置跟新
	fds[0].events = POLLIN;//#define POLLIN       (0x01)
///////////////////////大循环//////////////////////////////
	while (!_task_should_exit) {
/*****************第一部分：一系列的准备工作*****************/
		/* wait for up to 500ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}
///////////////////////获取各种信息///////////////////
		poll_subscriptions();

		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();//获取绝对时间，类似于时刻
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;//相对时间，类似时间段
		t_prev = t;//跟新t_prev

		// set dt for control blocks   给控制块设置dt(这个是操作系统层的)
		setDt(dt);
/////////////////////对解锁状态进行判断之后复位位置和高度设置/////////////////
		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			_vel_sp_prev.zero();
			reset_int_z = true;
			reset_int_xy = true;
			reset_yaw_sp = true;
		}
////////////////固定翼模式为垂直起降控制复位yaw和高度设置/////////////////////////
		/* reset yaw and altitude setpoint for VTOL which are in fw mode */
		if (_vehicle_status.is_vtol) {
			if (!_vehicle_status.is_rotary_wing) {
				reset_yaw_sp = true;
				_reset_alt_sp = true;
			}
		}
///////////////////跟新前一时刻的解锁状态/////////////////////////
		//Update previous arming state
		was_armed = _control_mode.flag_armed;

		update_ref();//跟新一些地坐标xyz方向基准值
//////////////////将之前获取的值赋给mc_pos_control_main.cpp里的变量///////////////		
//////////////////独立于当前模式跟新加速度//////////////////////
		/* Update velocity derivative,
		 * independent of the current flight mode
		 */
		if (_local_pos.timestamp > 0) {

			if (PX4_ISFINITE(_local_pos.x) &&
					PX4_ISFINITE(_local_pos.y) &&
					PX4_ISFINITE(_local_pos.z)) {
			/*ISFINITE是判断是否为有限数*/
				_pos(0) = _local_pos.x;
				_pos(1) = _local_pos.y;
				_pos(2) = _local_pos.z;
			}

			if (PX4_ISFINITE(_local_pos.vx) &&
					PX4_ISFINITE(_local_pos.vy) &&
					PX4_ISFINITE(_local_pos.vz)) {

				_vel(0) = _local_pos.vx;
				_vel(1) = _local_pos.vy;
				_vel(2) = _local_pos.vz;
			}

			_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));//微分
			_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
			_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
			/*	math::Vector<3> _vel_err_d;		//< derivative of current velocity */
			/*  _vel_err_d当前速度的导数*/
		}
////////////非手动模式下或者不需要位置高度控制的情况下，复位水平和垂直位置hold的标志位///////////////
////////////以下的_control_mode.xxxxxx均来自commander.cpp///////////////////////
		// reset the horizontal and vertical position hold flags for non-manual modes
		// or if position / altitude is not controlled
		if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
			_pos_hold_engaged = false;//不用控制位置||非手动
		}

		if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
			_alt_hold_engaged = false;//不用控制高度||非手动
		}
/*****************第二部分：这部分应该就是控制位置控制逻辑的集中体现了*****************/
////////高度控制、位置控制、爬升速率控制、速度控制任意一个使能则进入以下这段程序//////////
		if (_control_mode.flag_control_altitude_enabled ||
				_control_mode.flag_control_position_enabled ||
				_control_mode.flag_control_climb_rate_enabled ||
				_control_mode.flag_control_velocity_enabled) {

			_vel_ff.zero();//置零(有何用还没找到)
/////////////默认是位置/高度控制器，也可以直接运行速度控制器/////////////
			/* by default, run position/altitude controller. the control_* functions
			 * can disable this and run velocity controllers directly in this cycle */
			_run_pos_control = true;//标志位
			_run_alt_control = true;//标志位
/*****************第二部分第一步：产生位置auto/速度设定值manual&offboard(期望值)*****************/
////////////选择控制源是手动、机外(offboard)、还是自动控制，产生位置/速度设定值(期望值)//////////////////
////////////这部分的三个函数具体会在下面展开/////////////////////
			/* select control source */
			if (_control_mode.flag_control_manual_enabled) {
				/* manual control */
				control_manual(dt);
				_mode_auto = false;

			} else if (_control_mode.flag_control_offboard_enabled) {
				/* offboard control */
				control_offboard(dt);
				_mode_auto = false;

			} else {
				/* AUTO */
				control_auto(dt);
			}
/*****************第二部分第二步：产生姿态设定值(期望值)*****************/
////////////////////vtol不用管，它是用于固定翼的///////////////////
			/* weather-vane mode for vtol: disable yaw control */
			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
				_att_sp.disable_mc_yaw_control = true;

			} else {
				/* reset in case of setpoint updates */
				_att_sp.disable_mc_yaw_control = false;
			}
////////////////////工作在手动控制失能&&当前位置设定值合法&&当前位置设定值的类型是空闲状态下///////////////
////////////////////////////那么不运行控制器，并且设置油门为0/////////////////////////
			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
					&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
				/* idle state, don't run controller and set zero thrust */
			/*	static const uint8_t SETPOINT_TYPE_POSITION = 0;
			 *	static const uint8_t SETPOINT_TYPE_VELOCITY = 1;
			 *	static const uint8_t SETPOINT_TYPE_LOITER = 2;
			 *	static const uint8_t SETPOINT_TYPE_TAKEOFF = 3;
			 *	static const uint8_t SETPOINT_TYPE_LAND = 4;
			 *	static const uint8_t SETPOINT_TYPE_IDLE = 5;
			 *	static const uint8_t SETPOINT_TYPE_OFFBOARD = 6;
			 */
				R.identity();//R矩阵单位化
			/*	在大循环外定义了R矩阵
			 *	math::Matrix<3, 3> R;
			 *	R.identity();
			 *	
			 * Firmware/src/lib/mathlib/math/Matrix.hpp
			 * set identity matrix单位矩阵
			 *
			 * void identity(void) {
			 *	memset(data, 0, sizeof(data));
			 *	unsigned int n = (M < N) ? M : N;
			 *
			 *	for (unsigned int i = 0; i < n; i++)
			 *		data[i][i] = 1;
			 * }
			 */
				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
				//将姿态设定值的R_body[9]数组复制到R矩阵中
				_att_sp.R_valid = true;

				_att_sp.roll_body = 0.0f;
				_att_sp.pitch_body = 0.0f;
				_att_sp.yaw_body = _yaw;
				_att_sp.thrust = 0.0f;

				_att_sp.timestamp = hrt_absolute_time();
//////////////////////此处发布的姿态设定值不是正常使用的，都被置为0了//////////////////////////
				/* publish attitude setpoint */
				if (_att_sp_pub != nullptr) {
					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

				} else if (_attitude_setpoint_id) {
					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
				}

			}
////////////////手动控制使能&&着陆使能状态下///////////////////
			else if (_control_mode.flag_control_manual_enabled
					&& _vehicle_status.condition_landed) {
				//////不运行控制器，当着落的时候(所以位置和高度设定值等复位)//////
				/* don't run controller when landed */
				_reset_pos_sp = true;
				_reset_alt_sp = true;
				_mode_auto = false;
				reset_int_z = true;
				reset_int_xy = true;

				R.identity();//R矩阵单位化
				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
				//将姿态设定值的R_body[9]数组复制到R矩阵中
				_att_sp.R_valid = true;

				_att_sp.roll_body = 0.0f;
				_att_sp.pitch_body = 0.0f;
				_att_sp.yaw_body = _yaw;
				_att_sp.thrust = 0.0f;

				_att_sp.timestamp = hrt_absolute_time();
//////////////////这里发布的姿态设定值是和着陆模式有关的，并不是飞行的姿态设定值///////////////
				/* publish attitude setpoint */
				if (_att_sp_pub != nullptr) {
					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

				} else if (_attitude_setpoint_id) {
					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
				}

			}
/*****************第二部分第二步的重点：产生姿态设定值(期望值)*****************/
///////////////////这段程序应该才是发布正常飞行状态的姿态设定值//////////////////
///////////////////运行位置和高度控制器(否则采用已经计算出来的速度设定值)//////////////
			else {
			/****************第二部分第二步的重点(1):产生可利用的速度设定值(期望值)*******************/
				/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
				if (_run_pos_control) {
					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
				}

				if (_run_alt_control) {
					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
				}
			/*	刚进入大循环时赋值，进行选择
			 *	_run_pos_control = true;//标志位
			 *	_run_alt_control = true;//标志位
			 *
			 *	_pos(n)就是之前orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
			 *	_pos(0) = _local_pos.x;
			 *	_pos(1) = _local_pos.y;
			 *	_pos(2) = _local_pos.z;
			 *
			 *	_pos_sp就是之前control_manual(dt);control_offboard(dt);control_auto(dt);的输出值
			 */
				/* make sure velocity setpoint is saturated in xy*/
				float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
							  _vel_sp(1) * _vel_sp(1));

				if (vel_norm_xy > _params.vel_max(0)) {
					/* note assumes vel_max(0) == vel_max(1) */
					_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
					_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
				}
				//////////////////以下是设定垂直速度///////////////
				/* make sure velocity setpoint is saturated in z*/
				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));

				if (vel_norm_z > _params.vel_max(2)) {
					_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
				}

				if (!_control_mode.flag_control_position_enabled) {
					_reset_pos_sp = true;
				}

				if (!_control_mode.flag_control_altitude_enabled) {
					_reset_alt_sp = true;
				}

				if (!_control_mode.flag_control_velocity_enabled) {
					_vel_sp_prev(0) = _vel(0);
					_vel_sp_prev(1) = _vel(1);
					_vel_sp(0) = 0.0f;
					_vel_sp(1) = 0.0f;
					control_vel_enabled_prev = false;
				}

				if (!_control_mode.flag_control_climb_rate_enabled) {
					_vel_sp(2) = 0.0f;
				}
			/////////////////以下是起飞的垂直速度设定/////////////////
				/* use constant descend rate when landing, ignore altitude setpoint */
				/* 当着陆时，忽略高度设定值，用恒定的速度下降 */
				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
					_vel_sp(2) = _params.land_speed;
				}

				/* special thrust setpoint generation for takeoff from ground */
				/* 起飞时产生专用的推力设定值 */
				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
						&& _control_mode.flag_armed) {

					// check if we are not already in air.
					// if yes then we don't need a jumped takeoff anymore
					// 检查飞机是否在空中					
					if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
					// 是否在空中
						_takeoff_jumped = true;
					}

					if (!_takeoff_jumped) {
						// ramp thrust setpoint up
						// 阶梯式的提高推力设定值
						if (_vel(2) > -(_params.tko_speed / 2.0f)) {
							_takeoff_thrust_sp += 0.5f * dt;
							_vel_sp.zero();
							_vel_prev.zero();

						} else {
							// copter has reached our takeoff speed. split the thrust setpoint up
							// into an integral part and into a P part
							// 飞行器已达到起飞速度。将推力设定值分为积分和比例部分
							thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
							thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);//限幅
							_vel_sp_prev(2) = -_params.tko_speed;
							_takeoff_jumped = true;
							reset_int_z = false;
						}
					}

					if (_takeoff_jumped) {
						_vel_sp(2) = -_params.tko_speed;
					}
				///////////////以上是起飞垂直速度设定/////////////////////
				} else {
					_takeoff_jumped = false;
					_takeoff_thrust_sp = 0.0f;
				}
			/////////////////以上是起飞的垂直速度设定/////////////////


				// limit total horizontal acceleration
				// 限制水平加速度
				math::Vector<2> acc_hor;
				acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
				acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;

				if (acc_hor.length() > _params.acc_hor_max) {
					acc_hor.normalize();//标准化
					acc_hor *= _params.acc_hor_max;//限幅完成
					math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
					math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;//acc*dt+v_prev_sp
					_vel_sp(0) = vel_sp_hor(0);//修改限幅后的水平速度设定
					_vel_sp(1) = vel_sp_hor(1);//修改限幅后的水平速度设定
				}

				// limit vertical acceleration
				// 限制垂直加速度
				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

				if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
					acc_v /= fabsf(acc_v);//标准化
					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);//acc*dt+v_prev_sp
				}

				_vel_sp_prev = _vel_sp;

				_global_vel_sp.vx = _vel_sp(0);
				_global_vel_sp.vy = _vel_sp(1);
				_global_vel_sp.vz = _vel_sp(2);
				
				/* publish velocity setpoint */
				if (_global_vel_sp_pub != nullptr) {
					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);

				} else {
					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
				}
		/************************************************************************************************
		 * orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
		 * 经过control_manual(dt);control_offboard(dt);control_auto(dt);输出pos_sp
		 * 经过上部分输出_vel_sp
		 * 发布_global_vel_sp
		 ************************************************************************************************/
		 
		 /****************第二部分第二步的重点(2):产生可利用的推力定值(期望值)*******************/
////////////////////爬升速率控制使能||水平速度控制使能///////////////////////
				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
					/* reset integrals if needed */
					if (_control_mode.flag_control_climb_rate_enabled) {
					//爬升速率控制使能
						if (reset_int_z) {
							reset_int_z = false;
							float i = _params.thr_min;

							if (reset_int_z_manual) {
								i = _params.thr_hover;

								if (i < _params.thr_min) {
									i = _params.thr_min;

								} else if (i > _params.thr_max) {
									i = _params.thr_max;
								}
							}

							thrust_int(2) = -i;
						}

					} else {
						reset_int_z = true;
					}

					if (_control_mode.flag_control_velocity_enabled) {
					//水平速度控制使能
						if (reset_int_xy) {
							reset_int_xy = false;
							thrust_int(0) = 0.0f;
							thrust_int(1) = 0.0f;
						}

					} else {
						reset_int_xy = true;
					}

					/* velocity error */
					math::Vector<3> vel_err = _vel_sp - _vel;
					/* 	_vel是实际飞行器的速度
					 * 	_vel(0) = _local_pos.vx;
					 * 	_vel(1) = _local_pos.vy;
					 *	_vel(2) = _local_pos.vz;
					 *	struct vehicle_local_position_s			_local_pos;	
					 *	orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
					 */
					// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
					// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
					// 检查我们是否将非速度控制模式转变成速度控制模式，如果是，那么矫正xy速度设定值，以便姿态设定值是连续的
					if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {

						// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
						// given by the last attitude setpoint
						//矫正xy速度设定值
						_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
						_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
						_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
						_vel_sp_prev(0) = _vel_sp(0);
						_vel_sp_prev(1) = _vel_sp(1);
						_vel_sp_prev(2) = _vel_sp(2);
						control_vel_enabled_prev = true;

						// compute updated velocity error
						//用矫正后的速度设定值-实际速度，跟新速度误差
						vel_err = _vel_sp - _vel;
					}

					/* thrust vector in NED frame */
					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
					//推力设定值(三维)=速度差*P+速度差的差*D+积分
		/*********************************************************
		 ************上部分就将设定速度转变成设定推力*************
		 *********************************************************/			
					if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
							&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
						// for jumped takeoffs use special thrust setpoint calculated above
						thrust_sp.zero();
						thrust_sp(2) = -_takeoff_thrust_sp;
					}

					if (!_control_mode.flag_control_velocity_enabled) {
						thrust_sp(0) = 0.0f;
						thrust_arrayssp(1) = 0.0f;
					}

					if (!_control_mode.flag_control_climb_rate_enabled) {
						thrust_sp(2) = 0.0f;
					}

					/* limit thrust vector and check for saturation */
					/* 限制推力大小，检查是否饱和 */
					bool saturation_xy = false;
					bool saturation_z = false;

					/* limit min lift */
					//限制最小升力
					float thr_min = _params.thr_min;

					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
						/* don't allow downside thrust direction in manual attitude mode */
						//不允许下降推力在手动姿态模式
						thr_min = 0.0f;
					}

					float thrust_abs = thrust_sp.length();
					float tilt_max = _params.tilt_max_air;
					float thr_max = _params.thr_max;
					/* filter vel_z over 1/8sec */
					_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);//垂直速度低通滤波
					/* filter vel_z change over 1/8sec */
					float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
					_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;//垂直加速度低通滤波

					/* adjust limits for landing mode */
					/***********************着陆处理************************/
					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
							_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
						/* limit max tilt and min lift when landing */
						//降落时限制最大倾斜和最小升力
						tilt_max = _params.tilt_max_land;

						if (thr_min < 0.0f) {
							thr_min = 0.0f;
						}

						/* descend stabilized, we're landing */判断是否正在下降准备着陆
						if (!_in_landing && !_lnd_reached_ground
								&& (float)fabs(_acc_z_lp) < 0.1f
								&& _vel_z_lp > 0.5f * _params.land_speed) {
							_in_landing = true;
						}

						/* assume ground, cut thrust */
						//判断是否已经着陆
						if (_in_landing
								&& _vel_z_lp < 0.1f) {
							thr_max = 0.0f;
							_in_landing = false;
							_lnd_reached_ground = true;
						}

						/* once we assumed to have reached the ground always cut the thrust.
							Only free fall detection below can revoke this
						*/
						//如果已经着陆，那么切断推力。
						if (!_in_landing && _lnd_reached_ground) {
							thr_max = 0.0f;
						}

						/* if we suddenly fall, reset landing logic and remove thrust limit */
						// 如果突然下落，复位着陆的逻辑标志位并移除推力限制
						if (_lnd_reached_ground
								/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
								//假定自由落体加速度大于4米每秒的平方，速度 > 2.0f * _params.land_speed
								&& (_acc_z_lp > 4.0f
								    || _vel_z_lp > 2.0f * _params.land_speed)) {
							thr_max = _params.thr_max;
							_in_landing = false;
							_lnd_reached_ground = false;
						}

					} else {
						_in_landing = false;
						_lnd_reached_ground = false;
					}
					/***********************着陆处理完毕************************/
					/* limit min lift */
					//限制最小升力
					if (-thrust_sp(2) < thr_min) {
						thrust_sp(2) = -thr_min;
						saturation_z = true;
					}
					if (_control_mode.flag_control_velocity_enabled) {
						/* limit max tilt */
						//限制最大斜率(xy方向推力限幅)
						if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
							/* absolute horizontal thrust */
							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();

							if (thrust_sp_xy_len > 0.01f) {
								/* max horizontal thrust for given vertical thrust*/
								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);

								if (thrust_sp_xy_len > thrust_xy_max) {
									float k = thrust_xy_max / thrust_sp_xy_len;
									thrust_sp(0) *= k;
									thrust_sp(1) *= k;
									saturation_xy = true;
								}
							}
						}
					}

					if (_control_mode.flag_control_altitude_enabled) {
						/* thrust compensation for altitude only control modes */
						//推力补偿，用于高度控制
						float att_comp;

						if (_R(2, 2) > TILT_COS_MAX) {
							att_comp = 1.0f / _R(2, 2);

						} else if (_R(2, 2) > 0.0f) {
							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
							saturation_z = true;

						} else {
							att_comp = 1.0f;
							saturation_z = true;
						}

						thrust_sp(2) *= att_comp;
					}

					/* limit max thrust */
					//推力限幅
					thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */

					if (thrust_abs > thr_max) {
						if (thrust_sp(2) < 0.0f) {
							if (-thrust_sp(2) > thr_max) {
								/* thrust Z component is too large, limit it */
								thrust_sp(0) = 0.0f;
								thrust_sp(1) = 0.0f;
								thrust_sp(2) = -thr_max;
								saturation_xy = true;
								saturation_z = true;

							} else {
								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
								float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
								float k = thrust_xy_max / thrust_xy_abs;
								thrust_sp(0) *= k;
								thrust_sp(1) *= k;
								saturation_xy = true;
							}

						} else {
							/* Z component is negative, going down, simply limit thrust vector */
							float k = thr_max / thrust_abs;
							thrust_sp *= k;
							saturation_xy = true;
							saturation_z = true;
						}

						thrust_abs = thr_max;
					}

					/* update integrals */
					//跟新推力积分
					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
					}

					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;

						/* protection against flipping on ground when landing */
						if (thrust_int(2) > 0.0f) {
							thrust_int(2) = 0.0f;
						}
					}

					/* calculate attitude setpoint from thrust vector */
			/*********************第二部分第二步的重点(3)：根据推力向量计算姿态设定值(期望姿态)***********************/
			/*********************最后使用用于控制的四元数表达的旋转矩阵(旋转矩阵就是姿态)***********************/
					if (_control_mode.flag_control_velocity_enabled) {
						/* desired body_z axis = -normalize(thrust_vector) */
						/*************先求出body_x、body_y、body_z*****************/
						///////////body_x、body_y、body_z应该是方向余弦矩阵的三个列向量//////////////
						math::Vector<3> body_x;
						math::Vector<3> body_y;
						math::Vector<3> body_z;

						if (thrust_abs > SIGMA) {
							body_z = -thrust_sp / thrust_abs;//body_z矩阵是推力设定值矩阵的标准化

						} else {
							/* no thrust, set Z axis to safe value */
							body_z.zero();
							body_z(2) = 1.0f;
						}

						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
						//_att_sp.yaw_body = _pos_sp_triplet.current.yaw;(来自control_offboard和control_auto)
						//y_C相当于是矩阵(-sin(偏航角),cos(偏航角),0)
						if (fabsf(body_z(2)) > SIGMA) {
							/* desired body_x axis, orthogonal to body_z */
							body_x = y_C % body_z;//%是求叉积运算

							/* keep nose to front while inverted upside down */
							if (body_z(2) < 0.0f) {
								body_x = -body_x;
							}

							body_x.normalize();

						} else {
							/* desired thrust is in XY plane, set X downside to construct correct matrix,
							 * but yaw component will not be used actually */
							body_x.zero();
							body_x(2) = 1.0f;
						}

						/* desired body_y axis */
						body_y = body_z % body_x;
						/****************再求出R<3,3>矩阵******************/
						/* fill rotation matrix */
						for (int i = 0; i < 3; i++) {
							R(i, 0) = body_x(i);
							R(i, 1) = body_y(i);
							R(i, 2) = body_z(i);
						}

						/* copy rotation matrix to attitude setpoint topic */
						/****************将R<3,3>矩阵copy到_att_sp.R_body[]******************/
						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
						_att_sp.R_valid = true;

						/* copy quaternion setpoint to attitude setpoint topic */
						/****************由方向余弦旋转矩阵R得到四元数，并copy到att_sp.q_d[]****************/
						math::Quaternion q_sp;
						q_sp.from_dcm(R);
						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));

						/* calculate euler angles, for logging only, must not be used for control */
						/****************由旋转矩阵R得到姿态设置欧拉角，只是log调试用，不是给控制用的****************/
						math::Vector<3> euler = R.to_euler();
						_att_sp.roll_body = euler(0);
						_att_sp.pitch_body = euler(1);
						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
						//yaw已经用于构建原始矩阵，但实际上旋转矩阵在奇点附近是有区别的
					} else if (!_control_mode.flag_control_manual_enabled) {
						/* autonomous altitude control without position control (failsafe landing),
						 * force level attitude, don't change yaw */
						//没有位置控制的高度控制(故障安全降落)，固定水平姿态，不改变yaw角
						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);

						/* copy rotation matrix to attitude setpoint topic */
						//将旋转矩阵R<3,3>  copy到_att_sp.R_body[]
						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
						_att_sp.R_valid = true;

						/* copy quaternion setpoint to attitude setpoint topic */
						//由方向余弦旋转矩阵R得到四元数，并copy到_att_sp.q_d[]
						math::Quaternion q_sp;
						q_sp.from_dcm(R);
						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));

						_att_sp.roll_body = 0.0f;
						_att_sp.pitch_body = 0.0f;
					}

					_att_sp.thrust = thrust_abs;
					//推力向量的长度赋值给姿态推力设定值(att_sp.thrust)，这样才够各个方向力度分配

					/* save thrust setpoint for logging */
					//用于log，方便调试
					_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
					_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
					_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;

					_att_sp.timestamp = hrt_absolute_time();//测出用的绝对时间


				} else {
					reset_int_z = true;
				}
			}
	/*********************第三部分：将之前程序得到的各种信息填充_local_pos_sp结构体，并发布出去***********************/
			/* fill local position, velocity and thrust setpoint */
			_local_pos_sp.timestamp = hrt_absolute_time();
			_local_pos_sp.x = _pos_sp(0);
			_local_pos_sp.y = _pos_sp(1);
			_local_pos_sp.z = _pos_sp(2);
			//第二部分第一步：产生位置/速度设定值(期望值)
			_local_pos_sp.yaw = _att_sp.yaw_body;
			_local_pos_sp.vx = _vel_sp(0);
			_local_pos_sp.vy = _vel_sp(1);
			_local_pos_sp.vz = _vel_sp(2);
			//第二部分第二步的重点(1):产生可利用的速度设定值(期望值)
			/* publish local position setpoint */
			if (_local_pos_sp_pub != nullptr) {
				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);

			} else {
				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
			}

		}
////////高度控制、位置控制、爬升速率控制、速度控制的相关程序结束//////////
//////////////其他情况(位置控制失能)就复位各种设定值////////////
		else {
			/* position controller disabled, reset setpoints */
			_reset_alt_sp = true;
			_reset_pos_sp = true;
			_mode_auto = false;
			reset_int_z = true;
			reset_int_xy = true;
			control_vel_enabled_prev = false;

			/* store last velocity in case a mode switch to position control occurs */
			_vel_sp_prev = _vel;
		}
//////////////以下判断是并列于“高度控制、位置控制、爬升速率控制、速度控制”的判断////////////
//////////////所以会出现混控现象，在执行任务的时候还可以遥控控制/////////////////
//////////////手动控制和姿态控制都使能，则运行以下程序产生姿态设定值////////////////
		/* generate attitude setpoint from manual controls */
		if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {

			/* reset yaw setpoint to current position if needed */
			if (reset_yaw_sp) {
				reset_yaw_sp = false;
				_att_sp.yaw_body = _yaw;
			}

			/* do not move yaw while sitting on the ground */
			//当飞行器在地上是，不要移动yaw
			else if (!_vehicle_status.condition_landed &&
					!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {

				/* we want to know the real constraint, and global overrides manual */
				const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
							   _params.global_yaw_max;
				const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;

				_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;//旋转*旋转比率
				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);//期望yaw
				float yaw_offs = _wrap_pi(yaw_target - _yaw);//期望yaw-飞机yaw=需要调整的yaw
				//_wrap_pi()函数是将输入角度参数控制到(0,2*pi)
				
				// If the yaw offset became too big for the system to track stop
				// shifting it

				// XXX this needs inspection - probably requires a clamp, not an if
				if (fabsf(yaw_offs) < yaw_offset_max) {
					_att_sp.yaw_body = yaw_target;
				}
			}

			/* control roll and pitch directly if we no aiding velocity controller is active */
			if (!_control_mode.flag_control_velocity_enabled) {
				_att_sp.roll_body = _manual.y * _params.man_roll_max;
				_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
			}

			/* control throttle directly if no climb rate controller is active */
			//如果没有爬升速率控制器，则直接推力控制
			if (!_control_mode.flag_control_climb_rate_enabled) {
				float thr_val = throttle_curve(_manual.z, _params.thr_hover);//手动推力的转换，以便控制器输出控制推力力度
				_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());

				/* enforce minimum throttle if not landed */
				//如果没有着陆，则需要限制最小推力
				if (!_vehicle_status.condition_landed) {
					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
				}
			}

			math::Matrix<3, 3> R_sp;

			/* construct attitude setpoint rotation matrix */
			//构建姿态设定值的旋转矩阵并copy到_att_sp.R_body[]
			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));

			/* reset the acceleration set point for all non-attitude flight modes */
			//非姿态飞行模式，复位加速度设定值
			if (!(_control_mode.flag_control_offboard_enabled &&
					!(_control_mode.flag_control_position_enabled ||
					  _control_mode.flag_control_velocity_enabled))) {

				_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
			}

			/* copy quaternion setpoint to attitude setpoint topic */
			//将姿态设定值的旋转矩阵转换成四元数矩阵并copy到_att_sp.q_d[]，以便发布
			math::Quaternion q_sp;
			q_sp.from_dcm(R_sp);
			memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
			_att_sp.timestamp = hrt_absolute_time();

		}
///////////////手动控制部分结束///////////////////////
///////////////手动控制失能///////////////
		else {
			reset_yaw_sp = true;
		}
/////////////更新前一个时刻的速度用于D部分(D应该是PID的D)////////////////////
		/* update previous velocity for velocity controller D part */
		_vel_prev = _vel;
/////////////////发布姿态设定值///////////////////
/////////////////如果位置/速度失能而机外(offboard)使能，则不发布姿态设定值//////////////
/////////////////因为这种情况姿态设定值是通过mavlink应用发布的//////////////
/////////////////飞机工作于垂直起降或者做一个过渡，也不发布，因为此时由垂直起降控制部分发布/////////
		/* publish attitude setpoint
		 * Do not publish if offboard is enabled but position/velocity control is disabled,
		 * in this case the attitude setpoint is published by the mavlink app. Also do not publish
		 * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
		 * attitude setpoints for the transition).
		 */
		if (!(_control_mode.flag_control_offboard_enabled &&
				!(_control_mode.flag_control_position_enabled ||
				  _control_mode.flag_control_velocity_enabled))) {

			if (_att_sp_pub != nullptr) {
				orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

			} else if (_attitude_setpoint_id) {
				_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
			}
		}
///////////////////手动控制后复位高度控制的积分(悬停油门)，以便更好的转变为手动模式////////////////
		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
				     && !_control_mode.flag_control_climb_rate_enabled;
	}

	mavlink_log_info(_mavlink_fd, "[mpc] stopped");

	_control_task = -1;
}