void MulticopterPositionControl::control_auto(float dt)
{
///////////////复位位置设定值在自动模式下或者我们没有在动作控制模式(MC mode)下////////////	
	/* reset position setpoint on AUTO mode activation or if we are not in MC mode */
	if (!_mode_auto || !_vehicle_status.is_rotary_wing) {
		if (!_mode_auto) {
			_mode_auto = true;
		}

		_reset_pos_sp = true;
		_reset_alt_sp = true;

		reset_pos_sp();
		reset_alt_sp();
	}
//////////////////获取三重位置设定值//////////////////
	//Poll position setpoint
	bool updated;
	orb_check(_pos_sp_triplet_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
///////////////当前三重位置设定值是否为有限数，如果是则为有效值//////////////
		//Make sure that the position setpoint is valid
		if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
				!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
				!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
			_pos_sp_triplet.current.valid = false;
		}
	}
/////////////初始化标志位///////////////
	bool current_setpoint_valid = false;
	bool previous_setpoint_valid = false;

	math::Vector<3> prev_sp;
	math::Vector<3> curr_sp;
//////////////如果当前三重位置设定值合法,将当前设定值经纬度和高度转换成地坐标系的xyz值////////////
	if (_pos_sp_triplet.current.valid) {

		/* project setpoint to local frame */
		/*这个函数在此cpp里面经常使用，是将将经纬度转换成地坐标系xy值*/
		map_projection_project(&_ref_pos,
				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
				       &curr_sp.data[0], &curr_sp.data[1]);
		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);

		if (PX4_ISFINITE(curr_sp(0)) &&
				PX4_ISFINITE(curr_sp(1)) &&
				PX4_ISFINITE(curr_sp(2))) {
			current_setpoint_valid = true;
		}
	}
//////////////如果上一时刻三重位置设定值合法,将当前设定值经纬度和高度转换成地坐标系的xyz值////////////
	if (_pos_sp_triplet.previous.valid) {
		map_projection_project(&_ref_pos,
				       _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
				       &prev_sp.data[0], &prev_sp.data[1]);
		prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);

		if (PX4_ISFINITE(prev_sp(0)) &&
				PX4_ISFINITE(prev_sp(1)) &&
				PX4_ISFINITE(prev_sp(2))) {
			previous_setpoint_valid = true;
		}
	}
///////////////如果当前位置设定值合法/////////////////
	if (current_setpoint_valid) {
/*********************	下部分只是将设定值进行比例变换，缩小进一个区间   ******************/
		/////////////范围区间：位置误差导致的最大允许速度 为1，也就是说(0,1)之间///////////////
		/* scaled space: 1 == position error resulting max allowed speed */
		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
		/*用_params.pos_p的每一个元素除以_params.vel_max对应位置的每一个元素，结果赋值给scale
		 *	const Vector<N> edivide(const Vector<N> &v) const {
		 *		Vector<N> res;
		 *
		 *		for (unsigned int i = 0; i < N; i++)
		 *			res.data[i] = data[i] / v.data[i];
		 *
		 *		return res;
		 *	}
		 */
		////////////将当前设定值转换到范围区间中//////////////////
		/* convert current setpoint to scaled space */
		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
		/*	用curr_sp的每一个元素和scale对应位置的每一个元素相乘，结果赋值给curr_sp_s
		 *	Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
		 *	{
		 *		Matrix<Type, M, N> res;
		 *		const Matrix<Type, M, N> &self = *this;
		 *
		 *		for (size_t i = 0; i < M; i++) {
		 *			for (size_t j = 0; j < N; j++) {
		 *				res(i , j) = self(i, j)*other(i, j);
		 *			}
		 *		}
		 *
		 *		return res;
		 *	}
		 */
/*********************	上部分只是将设定值进行比例变换，缩小进一个区间   ******************/
///////////////////默认使用的当前设定值///////////////////////////
		/* by default use current setpoint as is */
		math::Vector<3> pos_sp_s = curr_sp_s;
//////////////////判断当前类型是否是位置设定类型&&上一次设定值是否合法////////////////////
		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
			/* follow "previous - current" line */
//////////////////遵守"previous - current"主线///////////////////
			if ((curr_sp - prev_sp).length() > MIN_DIST) {

				/* find X - cross point of unit sphere and trajectory */
				math::Vector<3> pos_s = _pos.emult(scale);//copy的_local_pos位置信息 * 比例
				math::Vector<3> prev_sp_s = prev_sp.emult(scale);//带了_s的都是乘以了比例的 scale
				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
				float curr_pos_s_len = curr_pos_s.length();
/////////////////根据飞行器位置距离当前位置设定点的距离分2种情况：小于单位半径和不小于单位半径////////////////////
////////////////////小于单位半径////////////////////
				if (curr_pos_s_len < 1.0f) {
					/* copter is closer to waypoint than unit radius */
					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
					/*飞行器距离航点在单位半径以内*/
					/*在奔向当前航点的时候检测下一个航点，避免通过当前航点时减速*/
					if (_pos_sp_triplet.next.valid) {
						math::Vector<3> next_sp;
						map_projection_project(&_ref_pos,
								       _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
								       &next_sp.data[0], &next_sp.data[1]);
						next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);

						if ((next_sp - curr_sp).length() > MIN_DIST) {
							math::Vector<3> next_sp_s = next_sp.emult(scale);

							/* calculate angle prev - curr - next */
							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
								/*标准化prev_curr_s
								 * returns the normalized version of this vector
								 *
								 *	Vector<N> normalized() const {
								 *		return *this / length();
								 *	}
								 */

							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
							/* prev_curr_s_norm是单位向量(当前位置设定-前一次位置设定)，curr_next_s是另一个向量(下一次位置设定-当前位置设定)
							 * 向量相乘点乘 ab=丨a丨丨b丨cosα，结果是一代数
							 * 向量相乘叉乘|向量c|=|向量a×向量b|=|a||b|sin，结果是一向量
							 */
							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;

							/* cos(b), b = angle pos - curr_sp - prev_sp */
							/* curr_pos_s是向量当前位置指向实际位置(实际位置-当前位置设定)
							 * prev_curr_s_norm是前一次位置设定指向当前位置设定的单位向量(当前位置设定-前一次位置设定)
							 * curr_pos_s_len是当前位置设定与实际位置之间长度
							 * 所以cos_b就是pos - curr_sp - prev_sp三点连线的角度的余弦值
							 */
							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;

							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {//a、b为锐角
								float curr_next_s_len = curr_next_s.length();

								/* if curr - next distance is larger than unit radius, limit it */
								/*当前位置设定到下个位置设定的距离超过单位半径*/
								if (curr_next_s_len > 1.0f) {
									cos_a_curr_next /= curr_next_s_len;//cos_a_curr_next=cos(a) * curr_next/||curr_next||
								}

								/* feed forward position setpoint offset */
								/*前馈位置设定值偏移*/
								math::Vector<3> pos_ff = prev_curr_s_norm *
											 cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
											 (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
								pos_sp_s += pos_ff;
							}
						}
					}

				}
////////////////////不小于单位半径////////////////////
				else {
					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);

					if (near) {
						/* unit sphere crosses trajectory */单位球越过轨迹

					} else {
						/* copter is too far from trajectory */
						/* if copter is behind prev waypoint, go directly to prev waypoint */
						/* 	飞行器离设定轨迹很远
						 *	如果飞行器在前一个设定位置后面，则先到前一个设定位置
						 *	角pos_sp - prev_sp - curr_sp大于90°
						 */
						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
							pos_sp_s = prev_sp_s;
						}

						/* if copter is in front of curr waypoint, go directly to curr waypoint */
						/* 如果飞行器在前一个设定位置前面，则到当前设定位置*/
						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
							pos_sp_s = curr_sp_s;
						}

						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
					}
				}
			}
		}
/* 由上述程序大概就可以看出控制逻辑
 * 先根据任务设定前一个、当前、下一个位置标定(prev_sp_s、curr_sp_s、next_sp_s)，用于控制飞行器按照此轨迹飞行
 * pos_sp_s是实时位置设定值，用于指导飞行器具体如何一个点一个点的靠近轨迹标定值(prev_sp_s、curr_sp_s、next_sp_s)
 * pos_s是飞行器实时位置
 * 带了_s的都是经过大小比例缩放的，不是实际值
 */
////////////////以下部分是限速用的//////////////
		/* move setpoint not faster than max allowed speed */
		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);

		/* difference between current and desired position setpoints, 1 = max speed */
		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
		float d_pos_m_len = d_pos_m.length();

		if (d_pos_m_len > dt) {
			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
		}

		/* scale result back to normal space */
		_pos_sp = pos_sp_s.edivide(scale);
////////////////以上部分是限速用的//////////////
		/* update yaw setpoint if needed */
		/* 跟新yaw的姿态设定值 */
		if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
		}
/////////////以下部分是用于起飞到位置巡航光滑切换///////////////////
		/*
		 * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
		 * this makes the takeoff finish smoothly.
		 */
		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
				|| _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
				&& _pos_sp_triplet.current.acceptance_radius > 0.0f
				/* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
				&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
			_reset_pos_sp = false;
			_reset_alt_sp = false;

			/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
			/* 在中断任务的情况下，不要去航点，但留在当前位置 */
		} else {
			_reset_pos_sp = true;
			_reset_alt_sp = true;
		}
/////////////以上部分是用于起飞到位置巡航光滑切换///////////////////
	} else {
		/* no waypoint, do nothing, setpoint was already reset */
	}
}
以上计算都是基于map_projection_project(&_ref_pos,sp.lat, sp.lon,&sp.data[0], &sp.data[1]);函数的计算(将经纬度转换成地坐标系xy值)也就是说是基于GPS的位置自动控制