好久没跟新blog了，这段时期边调试边看程序，所以有点慢。要开始着手调试了。。

这篇blog是顺着上一篇pixhawk 整体架构的认识写的，接下来看程序的话，打算把各个功能模块理解一遍，先从mc_pos_control.cpp看起。

先提一下pixhawk的整体逻辑：

commander和navigator产生期望位置
position_estimator是当前位置
通过pos_ctrl产生期望姿态
attitude_estimator是当前姿态
通过att_ctrl产生pwm的数值
最后通过mixer和motor_driver控制4个电机

pos_ctrl的总体逻辑是：

（1）copy  commander和navigator产生的期望位置-----_pos_sp_triplet结构体

（2）产生位置/速度设定值(期望值)-----_pos_sp<3>向量和_vel_sp<3>向量

（3）产生可利用的速度设定值(期望值)-----_vel_sp<3>向量

（4）产生可利用的推力定值(期望值)-----thrust_sp<3>向量

（5）根据推力向量计算姿态设定值(期望姿态)-----q_sp四元数矩阵和R_sp旋转矩阵

（6）将之前程序得到的各种信息填充_local_pos_sp结构体，并发布出去-----_local_pos_sp（第2、3步得到的）

（7）根据具体应用更改之前得到的姿态设定值(期望姿态)，并发布出去-----_att_sp（第5步得到的）

那么，直接贴代码了，代码中有详细注释（比之前要更详细点，里面细节逻辑太多了，还没完全理清）

先是main代码，再是control_manual(dt);control_offboard(dt);control_auto(dt);函数，再是map_projection_project()函数，最后是常用矩阵和向量函数

task_main()

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;//标志位
/*****************第二部分第一步：产生位置/速度设定值(期望值)*****************/
选择控制源是手动、机外(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;
}

control_manual()函数

void
MulticopterPositionControl::control_manual(float dt)
{
	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
	req_vel_sp.zero();

	if (_control_mode.flag_control_altitude_enabled) {
		/* set vertical velocity setpoint with throttle stick */
		/* 将自稳模式的油门杆转换成控制垂直速度设定值(以中间速度为0，往上拨速度向上，往下拨速度向下，速度大小与拨动幅度成正比) */
		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
	}

	if (_control_mode.flag_control_position_enabled) {
		/* set horizontal velocity setpoint with roll/pitch stick */
		/* 水平速度设定值由roll和pitch杆确定 */
		req_vel_sp(0) = _manual.x;
		req_vel_sp(1) = _manual.y;
	}

	if (_control_mode.flag_control_altitude_enabled) {
		/* reset alt setpoint to current altitude if needed */
		reset_alt_sp();//复位高度设定值
	}

	if (_control_mode.flag_control_position_enabled) {
		/* reset position setpoint to current position if needed */
		reset_pos_sp();//复位位置设定值
	}
//以下限制速度设定值///
	/* limit velocity setpoint */
	float req_vel_sp_norm = req_vel_sp.length();

	if (req_vel_sp_norm > 1.0f) {
		req_vel_sp /= req_vel_sp_norm;
	}
//以上限制速度设定值///
	/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
	math::Matrix<3, 3> R_yaw_sp;
	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);//由欧拉角得到旋转矩阵
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * create a rotation matrix from given euler angles
	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
	 *
	 *	void from_euler(float roll, float pitch, float yaw) {
	 *		float cp = cosf(pitch);
	 *		float sp = sinf(pitch);
	 *		float sr = sinf(roll);
	 *		float cr = cosf(roll);
	 *		float sy = sinf(yaw);
	 *		float cy = cosf(yaw);
	 *
	 *		data[0][0] = cp * cy;
	 *		data[0][1] = (sr * sp * cy) - (cr * sy);
	 *		data[0][2] = (cr * sp * cy) + (sr * sy);
	 *		data[1][0] = cp * sy;
	 *		data[1][1] = (sr * sp * sy) + (cr * cy);
	 *		data[1][2] = (cr * sp * sy) - (sr * cy);
	 *		data[2][0] = -sp;
	 *		data[2][1] = sr * cp;
	 *		data[2][2] = cr * cp;
	 *	}
	 */
	math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
			_params.vel_max); // in NED and scaled to actual velocity
							  // 在NED坐标系下，还原到真实的速度
/以下为水平轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/
	/*
	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
	 * hold is activated for the corresponding axis
	 */
	/* horizontal axes */
	/* 水平轴 */
	if (_control_mode.flag_control_position_enabled) {
		/* check for pos. hold */
		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
			if (!_pos_hold_engaged) {
				if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
						&& fabsf(_vel(1)) < _params.hold_max_xy)) {
					_pos_hold_engaged = true;

				} else {
					_pos_hold_engaged = false;
				}
			}

		} else {
			_pos_hold_engaged = false;
		}
/以上为辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/
/以下为速度设定值，作为此函数的输出///
		/* set requested velocity setpoint */
		if (!_pos_hold_engaged) {//不需要位置锁定(辅助速度模式)
			_pos_sp(0) = _pos(0);
			_pos_sp(1) = _pos(1);
			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
									  /* 用于速度设定而不是位置设定 */
			_vel_sp(0) = req_vel_sp_scaled(0);
			_vel_sp(1) = req_vel_sp_scaled(1);
		}
	}
以下为垂直轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/
	/* vertical axis */
	if (_control_mode.flag_control_altitude_enabled) {
		/* check for pos. hold */
		if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
			if (!_alt_hold_engaged) {
				if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
					_alt_hold_engaged = true;

				} else {
					_alt_hold_engaged = false;
				}
			}

		} else {
			_alt_hold_engaged = false;
		}
以上为垂直轴辅助速度模式：用户控制速度，但是如果速度很小，那么相应轴上的位置保持不变/
		/* set requested velocity setpoint */
		if (!_alt_hold_engaged) {
			_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
									  /* 用于速度设定而不是位置设定 */
			_vel_sp(2) = req_vel_sp_scaled(2);
			_pos_sp(2) = _pos(2);
		}
	}
}

control_offboard()函数

void
MulticopterPositionControl::control_offboard(float dt)
{
	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);
	}
//水平轴设定//
	if (_pos_sp_triplet.current.valid) {
		if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
		//控制模式-位置控制使能&&当前位置设定值合法，那么进行位置控制
			/* control position */
			_pos_sp(0) = _pos_sp_triplet.current.x;
			_pos_sp(1) = _pos_sp_triplet.current.y;

		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
		//控制模式-速度控制使能&&当前速度设定值合法
			/* control velocity */
			/* reset position setpoint to current position if needed */
			reset_pos_sp();//速度控制时，需要复位位置

			/* set position setpoint move rate */
			_vel_sp(0) = _pos_sp_triplet.current.vx;
			_vel_sp(1) = _pos_sp_triplet.current.vy;

			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
		}
yaw姿态设定///
		if (_pos_sp_triplet.current.yaw_valid) {
			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;

		} else if (_pos_sp_triplet.current.yawspeed_valid) {
			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
		}
/垂直轴设定///
		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
		//控制模式：高度控制模式&&当前位置设定是否合法
			/* Control altitude */
			_pos_sp(2) = _pos_sp_triplet.current.z;

		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
		//控制模式：爬升速度控制模式&&当前速度设定是否合法
			/* reset alt setpoint to current altitude if needed */
			reset_alt_sp();

			/* set altitude setpoint move rate */
			_vel_sp(2) = _pos_sp_triplet.current.vz;

			_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
		}

	} else {
		reset_pos_sp();
		reset_alt_sp();
	}
}

control_auto()函数

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的位置自动控制

map_projection_project()函数

map_projection_project(&_ref_pos,
				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
				       &curr_sp.data[0], &curr_sp.data[1]);
					   
输入参数：	&_ref_pos
				/* lat/lon are in radians */
				struct map_projection_reference_s {//地图投影参考
					double lat_rad;
					double lon_rad;
					double sin_lat;
					double cos_lat;
					bool init_done;
					uint64_t timestamp;
				};
				纬度是地心到某地连线与地心到赤道连线的夹角就是某地纬度，赤道为0度，两极是90度
				经度是英国伦敦附近的格林尼治天文台为0度，向东西两边逐加分别为东经和西经，到背面就是180度
				度数/360=弧度/2π
			_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon
				position_setpoint_s结构体中double lat;(纬度)double lon;(经度)，由navigator发布
			&curr_sp.data[0], &curr_sp.data[1]
				x,y方向位置

__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
				    float *y)
输入参数：	*ref
				/* lat/lon are in radians */
				struct map_projection_reference_s {//地图投影参考
					double lat_rad;
					double lon_rad;
					double sin_lat;
					double cos_lat;
					bool init_done;
					uint64_t timestamp;
				};
				纬度是地心到某地连线与地心到赤道连线的夹角就是某地纬度，赤道为0度，两极是90度
				经度是英国伦敦附近的格林尼治天文台为0度，向东西两边逐加分别为东经和西经，到背面就是180度
				度数/360=弧度/2π
			double lat, double lon,
				double lat;(纬度)double lon;(经度)
			float *x, float *y
				x,y方向位置			
实现的功能：将经纬度转换成地坐标系xy值				
{
	if (!map_projection_initialized(ref)) {
		return -1;
	}

	double lat_rad = lat * M_DEG_TO_RAD;//#define M_DEG_TO_RAD 	0.01745329251994角度转弧度
	double lon_rad = lon * M_DEG_TO_RAD;//#define M_RAD_TO_DEG 	57.2957795130823弧度转角度

	double sin_lat = sin(lat_rad);
	double cos_lat = cos(lat_rad);
	double cos_d_lon = cos(lon_rad - ref->lon_rad);
	//ref->lon_rad是copy ORB_ID(vehicle_local_position)主题，经过update_ref()里面的
	//map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
	//ref->lon_rad = lon_0 * M_DEG_TO_RAD;

	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
	double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));

	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
	*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
	//#define CONSTANTS_RADIUS_OF_EARTH(地球半径)			6371000			/* meters (m)		*/
	return 0;
}

常用矩阵向量函数

	/**Firmware/src/lib/mathlib/math/Quaternion.hpp
	 * create rotation matrix for the quaternion
	 */由四元数得到方向余弦旋转矩阵
	Matrix<3, 3> to_dcm(void) const {
		Matrix<3, 3> R;
		float aSq = data[0] * data[0];
		float bSq = data[1] * data[1];
		float cSq = data[2] * data[2];
		float dSq = data[3] * data[3];
		R.data[0][0] = aSq + bSq - cSq - dSq;
		R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
		R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
		R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
		R.data[1][1] = aSq - bSq + cSq - dSq;
		R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
		R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
		R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
		R.data[2][2] = aSq - bSq - cSq + dSq;
		return R;
	}

	/**Firmware/src/lib/mathlib/math/Quaternion.hpp
	 * set quaternion to rotation by DCM
	 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
	 */由方向余弦旋转矩阵得到四元数
	void from_dcm(const Matrix<3, 3> &dcm) {
		float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
		if (tr > 0.0f) {
			float s = sqrtf(tr + 1.0f);
			data[0] = s * 0.5f;
			s = 0.5f / s;
			data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
			data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
			data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
		} else {
			/* Find maximum diagonal element in dcm
			* store index in dcm_i */
			int dcm_i = 0;
			for (int i = 1; i < 3; i++) {
				if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
					dcm_i = i;
				}
			}
			int dcm_j = (dcm_i + 1) % 3;
			int dcm_k = (dcm_i + 2) % 3;
			float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
			dcm.data[dcm_k][dcm_k]) + 1.0f);
			data[dcm_i + 1] = s * 0.5f;
			s = 0.5f / s;
			data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
			data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
			data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
		}
	}

	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * get euler angles from rotation matrix
	 */由旋转矩阵得到欧拉角
	Vector<3> to_euler(void) const {
		Vector<3> euler;
		euler.data[1] = asinf(-data[2][0]);

		if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];

		} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];

		} else {
			euler.data[0] = atan2f(data[2][1], data[2][2]);
			euler.data[2] = atan2f(data[1][0], data[0][0]);
		}

		return euler;
	}
	
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * set zero matrix  零矩阵
	 */
	void zero(void) {
		memset(data, 0, sizeof(data));
	}
	
	/**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;
	}

typedef struct pollfd px4_pollfd_struct_t;
	
/* In the standard poll() definition, the size of the event set is 'short'.
 * Here we pick the smallest storage element that will contain all of the
 * poll events.
 */
/* 在标准轮询（）的定义，设置事件的大小是“short”。 在这里，最小存储元件将包含所有的轮训事件*/
typedef uint8_t pollevent_t;

/* This is the Nuttx variant of the standard pollfd structure. */
/*这是标准的pollfd结构的Nuttx变量*/
struct pollfd
{
  int         fd;       /* The descriptor being polled */
  sem_t      *sem;      /* Pointer to semaphore used to post output event */
  pollevent_t events;   /* The input event flags */
  pollevent_t revents;  /* The output event flags */
  FAR void   *priv;     /* For use by drivers */
};

	/**Firmware/src/lib/mathlib/math/Vector.hpp
	 * element by element multiplication
	 */
    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;
    }
	
	/**Firmware/src/lib/mathlib/math/Vector.hpp
	 * element by element division
	 */
	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;
	}
		
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * create a rotation matrix from given euler angles
	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
	 */由欧拉角得到旋转矩阵
	void from_euler(float roll, float pitch, float yaw) {
		float cp = cosf(pitch);
		float sp = sinf(pitch);
		float sr = sinf(roll);
		float cr = cosf(roll);
		float sy = sinf(yaw);
		float cy = cosf(yaw);

		data[0][0] = cp * cy;
		data[0][1] = (sr * sp * cy) - (cr * sy);
		data[0][2] = (cr * sp * cy) + (sr * sy);
		data[1][0] = cp * sy;
		data[1][1] = (sr * sp * sy) + (cr * cy);
		data[1][2] = (cr * sp * sy) - (sr * cy);
		data[2][0] = -sp;
		data[2][1] = sr * cp;
		data[2][2] = cr * cp;
	}

如果您觉得此文对您的发展有用，请随意打赏。 
您的鼓励将是笔者书写高质量文章的最大动力^_^!!