PX4多旋翼姿态控制程序分析mc_att_control

PX4多旋翼姿态控制程序分析mc_att_control

1. 程序流程图

原图大小太大，无法上传（在我主页下载里面找）

2.PX4主程序梳理：建议对照上图

PX4程序的基本流程如下：

int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
{
	bool vtol = false;
	// 判断是否是VTOL飞行器
	if (argc > 1) {
		if (strcmp(argv[1], "vtol") == 0) {
			vtol = true;
		}
	// 否则创建的任务将是普通的多旋翼飞行器
	}

	MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol);
	// 实例初始化
	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;
		
		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}
	// 创建任务失败或初始化失败时，清除实例并释放任务所占用的内存
	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
	// 根据vtol来决定发布主题
	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
	_vtol(vtol)
{
	// 多旋翼飞行器为VTOL类型
	if (_vtol) {
		int32_t vt_type = -1;
		// 查找参数名为VT_TYPE的参数，将参数值赋值给变量vt_type
		if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
			// 将vt_type的值转换为vtol_type枚举类型，并与TAILSITTER进行比较
			// 如果相等，则说明多旋翼飞行器为垂尾式VTOL
			_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
		}
	}
	// 参数更新
	parameters_updated();
}

bool MulticopterAttitudeControl::init()
{
	// 注册了一个 姿态信息订阅器的回调函数
	if (!_vehicle_attitude_sub.registerCallback()) {
		PX4_ERR("vehicle_attitude callback registration failed!");
		return false;
	}

	return true;
}

3.Run()函数解析

// 检查是否需要退出程序
if (should_exit()) {
		_vehicle_attitude_sub.unregisterCallback();// 取消姿态信息订阅器的回调函数
		exit_and_cleanup();// 执行清理函数
		return;
	}
	// 启动计时
	perf_begin(_loop_perf);

	// 检查参数是否发生变化
	if (_parameter_update_sub.updated()) {
		// clear update
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		// 如果有变化就更新参数
		updateParams();
		//通知其他的函数参数已经被更新。这个函数通常在初始化时调用一次，之后会在循环中不断检查参数是否有变化，并且及时更新参数
		parameters_updated();
	}

parameter_update_s结构体包含了各种参数更新相关的信息:

struct __EXPORT parameter_update_s {
#else
struct parameter_update_s {
#endif
	uint64_t timestamp;		// 更新时的时间戳
	uint32_t instance;		// 参数的实例编号
	uint32_t get_count;		// 获取参数的次数
	uint32_t set_count;		// 设置参数的次数
	uint32_t find_count;	// 查找参数的次数
	uint32_t export_count;	// 导出参数的次数
	uint16_t active;		// 参数是否激活的标志
	uint16_t changed;		// 参数是否发生变化的标志
	uint16_t custom_default;// 是否使用自定义的默认值的标志
	uint8_t _padding0[6]; // required for logger

#ifdef __cplusplus

#endif
};

// run controller on attitude updates
vehicle_attitude_s v_att;
// 存储飞行器姿态数据 结构体
struct vehicle_attitude_s {
#endif
	uint64_t timestamp;			// 时间戳
	uint64_t timestamp_sample;	// 采样时间戳	
	float q[4];				   // 飞行器当前的四元数（即姿态）
	float delta_q_reset[4];		// 飞行器重置后的姿态变化量（四元数表示）
	uint8_t quat_reset_counter;	// 飞行器姿态重置次数
	uint8_t _padding0[7]; // required for logger
#ifdef __cplusplus
#endif
};

// 这是一个存储飞行器姿态期望数据的结构体
struct vehicle_attitude_setpoint_s {
#endif
	uint64_t timestamp;			// 姿态期望数据的时间戳
    
	float roll_body;			// 期望的机身坐标系下的横滚角
	float pitch_body;
	float yaw_body;
	float yaw_sp_move_rate;
    
	float q_d[4];				// 期望的四元数姿态：期望的旋转方向和角度大小
	float thrust_body[3];		 // 期望的机身坐标系下的推力向量
    
	bool roll_reset_integral;	 // 表示是否需要重置横滚角的积分项
	bool pitch_reset_integral;
	bool yaw_reset_integral;
    
	bool fw_control_yaw;		// 表示是否使用飞翼模式下的偏航控制
	uint8_t apply_flaps;		// 当前的襟翼状态，可能的取值为 FLAPS_OFF（襟翼关闭）、FLAPS_LAND（着陆襟翼）、FLAPS_TAKEOFF（起飞襟翼）
	uint8_t _padding0[7]; // required for logger
#ifdef __cplusplus
	static constexpr uint8_t FLAPS_OFF = 0;
	static constexpr uint8_t FLAPS_LAND = 1;
	static constexpr uint8_t FLAPS_TAKEOFF = 2;
#endif
};

// 获取最新的飞行器姿态数据
if (_vehicle_attitude_sub.update(&v_att)) {
    // 检查是否有新的姿态设定值更新
    if (_vehicle_attitude_setpoint_sub.updated()) {
        // 如果有新的设定值,则从_vehicle_attitude_setpoint_sub订阅的话题中获取最新的设定值
        vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
        _vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
        
        // 姿态设定:四元数（期望的旋转方向和角度大小），期望的偏航速率
        _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
        
        // 将设定值中的机体坐标系推力矢量（thrust_body）转换成向量（Vector3f）
        _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
    }
    // 检查飞机姿态是否进行了重置
    if (_quat_reset_counter != v_att.quat_reset_counter) {
        const Quatf delta_q_reset(v_att.delta_q_reset);
        
        // 如果重置，就将重置带来的航向变化量添加到期望的航向角度
        _man_yaw_sp += Eulerf(delta_q_reset).psi();// 转换为欧拉角
        _attitude_control.adaptAttitudeSetpoint(delta_q_reset);
        _quat_reset_counter = v_att.quat_reset_counter;
    }
    // 计算时间步长dt
    const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f);
    _last_run = v_att.timestamp;
    
    /* check for updates in other topics */
    _manual_control_setpoint_sub.update(&_manual_control_setpoint);	// 获取手动控制指令
    _v_control_mode_sub.update(&_v_control_mode);// 获取飞行模式

    // 检查否有新的更新
    if (_vehicle_land_detected_sub.updated()) {
        vehicle_land_detected_s vehicle_land_detected;
		// 是否成功从主题中复制了最新的消息
        if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
            _landed = vehicle_land_detected.landed;	//提取出是否着陆的信息
        }
    }

struct manual_control_setpoint_s {
	uint64_t timestamp;
	uint64_t timestamp_sample;
	float x;	// 遥控 油门输入，用于控制飞机的前进速度
	float y;	// 横滚输入，用于控制飞机的左右滚转角度
	float z;	// 俯仰输入，用于控制飞机的上下俯仰角度
	float r;	// 偏航输入，用于控制飞机绕垂直轴旋转的角度
	float flaps;// 襟翼输入，用于改变飞机的升力和阻力，控制飞机的下降速率和着陆方式等
	float aux1; // 额外辅助输入，用于控制飞机的其他功能，例如开启/关闭飞机的自动巡航功能等
	float aux2;
	float aux3;
	float aux4;
	float aux5;
	float aux6;
	uint8_t data_source;// 手动遥控信号的来源。可以是手动遥控器本身，也可以是来自其他设备的信号。
	uint8_t _padding0[3]; // required for logger
};

// 检测无人机状态是否有新的更新
if (_vehicle_status_sub.updated()) {
    vehicle_status_s vehicle_status;
	// // 是否成功从主题中复制了最新的消息
    if (_vehicle_status_sub.copy(&vehicle_status)) {
        
        // 记录当前无人机是否为旋翼机型
        _vehicle_type_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
        // 记录当前无人机是否为 VTOL（垂直起降固定翼）
        _vtol = vehicle_status.is_vtol;
        // 记录当前无人机是否处于转换模式（即从一种飞行模式切换到另一种飞行模式的过程中）
        _vtol_in_transition_mode = vehicle_status.in_transition_mode;
    }
}

vehicle_status_s用于存储无人机的状态信息

struct vehicle_status_s {
#endif
	uint64_t timestamp;				// 系统时间戳
	uint64_t nav_state_timestamp;	 // 最后一次导航状态的时间戳
	uint64_t failsafe_timestamp;	 // 最后一次进入失控保护状态的时间戳
	uint64_t armed_time;			// 解锁时间
	uint64_t takeoff_time;			// 起飞时间
	uint32_t onboard_control_sensors_present;	// 当前连接的传感器掩码
	uint32_t onboard_control_sensors_enabled;	// 当前启用的传感器掩码
	uint32_t onboard_control_sensors_health;	// 当前连接的传感器状态掩码。
	uint8_t nav_state;				// 当前导航状态
	uint8_t arming_state;			// 当前解锁状态
	uint8_t hil_state;				// 硬件在环仿真(Hardware-in-the-Loop Simulation)状态
	bool failsafe;				    // 是否处于失控保护状态。
	uint8_t system_type;			// 类型(例如：固定翼、多旋翼、无人船)
	uint8_t system_id;				// 系统ID
	uint8_t component_id;			// 组件ID
	uint8_t vehicle_type;			// 类型(例如：固定翼、多旋翼、无人船)
	bool is_vtol;					// 是否为垂直起降固定翼
	bool is_vtol_tailsitter;		// 是否为尾垂式VTOL
	bool vtol_fw_permanent_stab;	// VTOL前飞行时是否使用永久稳定器
	bool in_transition_mode;		// 载具是否处于转换模式
	bool in_transition_to_fw;		// 载具是否从垂直起降模式转换到了固定翼模式
	bool rc_signal_lost;			// 是否失去了遥控信号
	uint8_t rc_input_mode;			// 当前遥控输入模式
	bool data_link_lost;			// 是否失去了数据链路信号
	uint8_t data_link_lost_counter;	 // 数据链路丢失的次数
	bool high_latency_data_link_lost;// 是否失去了高延迟数据链路信号
	bool engine_failure;			// 是否发生了引擎故障
	bool mission_failure;			// 是否发生了任务失败
	bool geofence_violated;			// 是否越界
	uint8_t failure_detector_status; // 故障检测器状态
	uint8_t latest_arming_reason;	
	uint8_t latest_disarming_reason;
	uint8_t _padding0[4]; // required for logger
}

		
		bool attitude_setpoint_generated = false;

		// 当前无人机正处于悬停模式 ( 当前飞行器类型为旋翼机 && 当前飞行器不处于转换模式)
		const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);

		// 当前飞行器是否为尾翼式垂直起降飞行器并且处于转换模式
		const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
	
		// 是否启用了姿态控制 并且 当前无人机正处于悬停模式或者当前飞行器是否为尾翼式垂直起降飞行器并且处于转换模式
		bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
		// 是否需要执行姿态控制
		if (run_att_ctrl) {

			const Quatf q{v_att.q};

			// 判断当前控制模式为手动（Manual）或者稳定（Stabilized）模式，且没有开启高度、速度、位置控制
			if (_v_control_mode.flag_control_manual_enabled &&
			    !_v_control_mode.flag_control_altitude_enabled &&
			    !_v_control_mode.flag_control_velocity_enabled &&
			    !_v_control_mode.flag_control_position_enabled) {
				
                  // 根据手柄输入和当前姿态 生成姿态设定点
				generate_attitude_setpoint(q, dt, _reset_yaw_sp);
                 // 表示已经生成了姿态设定点
				attitude_setpoint_generated = true;

			} else {	//否则将手柄输入滤波器的状态重置为0
				_man_x_input_filter.reset(0.f);
				_man_y_input_filter.reset(0.f);
			}
			
             // update()函数返回一个计算出的期望角速度向量rates_sp
             // 参数q是当前飞行器的姿态四元数。通过将当前飞行器的姿态四元数和期望姿态四元数（在之前的代码中计算）一起传递给姿态控制器，它计算出旋转向量（旋转速度），并将其转换为期望角速度向量，即返回的rates_sp
			Vector3f rates_sp = _attitude_control.update(q);

			// 发布无人机期望的姿态速率（角速度）和推力信息
			vehicle_rates_setpoint_s v_rates_sp{};
			v_rates_sp.roll = rates_sp(0);
			v_rates_sp.pitch = rates_sp(1);
			v_rates_sp.yaw = rates_sp(2);
			_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
			v_rates_sp.timestamp = hrt_absolute_time();

			_v_rates_sp_pub.publish(v_rates_sp);
		}

		// reset yaw setpoint during transitions, tailsitter.cpp generates
		// attitude setpoint for the transition

		// 用于在姿态控制中 generate_attitude_setpoint（） 指示是否需要重置偏航设定点
		// 当attitude_setpoint_generated为false（还未生成新的姿态设定点），或者飞机降落（即_landed为true），或者垂直起降机处于转换模式（即_vtol和_vtol_in_transition_mode都为true）时，就将_reset_yaw_sp设置为true。
		_reset_yaw_sp = !attitude_setpoint_generated || _landed || (_vtol && _vtol_in_transition_mode);
	}
	//  结束计时器
	perf_end(_loop_perf);
}

4.计算期望姿态角

该函数将一个四元数表示的当前姿态和一个时间步长作为输入，并根据遥控器输入和其它参数生成新的姿态设定点。**函数的主要任务是将遥控器输入的横滚和俯仰信号映射到机体坐标系中的横滚和俯仰角，并将遥控器输入的偏航信号转化为偏航速度设定值。**同时，它还会对姿态角进行限制，以避免出现机体不稳定的情况。

void MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp)
{
	vehicle_attitude_setpoint_s attitude_setpoint{};
    
/*当前偏航角计算-----------------------------------------------------------------------------------------------------------------------*/
	const float yaw = Eulerf(q).psi();

	/* reset yaw setpoint to current position if needed */
	if (reset_yaw_sp) {
		_man_yaw_sp = yaw;	// 将遥控器输入的偏航信号设为当前位置

	} else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f		/* 检查手动控制输入信号的高度 是否大于 0.05f */
		   || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {	// 或飞行器是否处于 roll_pitch_yaw 模式（在这种模式下，飞行器的横滚、俯仰和偏航姿态受到手动遥控器输入的控制，这种模式适用于需要更精细控制的飞行，例如飞行器需要保持特定的朝向进行视觉或传感器任务时。）
		const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
		attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;	// 手动控制输入的偏航量 * 偏航速率限制 = 偏航角速度设定值
		_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);// 乘以 dt 并加上当前的 偏航角，最后使用 wrap_pi 函数将结果限制在 [-π, π] 
	}

	/*
	 * Input mapping for roll & pitch setpoints
	 * ----------------------------------------
	 * We control the following 2 angles:
	 * - tilt angle, given by sqrt(x*x + y*y)
	 * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion
	 *
	 * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
	 * points to, and changes of the stick input are linear.
	 */
	_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
	_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
    
    // 控制的倾斜角度最大值
	_man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
	_man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max);
	const float x = _man_x_input_filter.getState();
	const float y = _man_y_input_filter.getState();

	// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane
    // 使用更新后的摇杆信号 x 和 y，创建一个二维向量 v，表示无人机在 XY 平面上的倾斜方向。
    // 为了使无人机朝向这个方向，我们使用 v 的法向量作为无人机的滚转和俯仰轴
	Vector2f v = Vector2f(y, -x);
    
    // 计算向量 v 的模来获取倾斜角度
	float v_norm = v.norm(); // the norm of v defines the tilt angle

	if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
		v *= _man_tilt_max / v_norm;
	}

	Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f);	// 计算出一个旋转角度
	Eulerf euler_sp = q_sp_rpy;					   // 转化为欧拉角
    
    // v的x和y坐标分别对应飞机在滚转和俯仰轴上的倾斜角度
    // euler_sp(2)对应的是旋转后的姿态中无人机的偏航角；然后将yaw的值设为_man_yaw_sp加上旋转后的姿态中的偏航角
    // 这样，就得到了完整的飞行姿态控制指令。
	attitude_setpoint.roll_body = euler_sp(0);
	attitude_setpoint.pitch_body = euler_sp(1);
	// The axis angle can change the yaw as well (noticeable at higher tilt angles).
	// This is the formula by how much the yaw changes:
	//   let a := tilt angle, b := atan(y/x) (direction of maximum tilt)
	//   yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).
	attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); 	

	/* modify roll/pitch only if we're a VTOL */
    // 这段代码是用于处理 VTOL（垂直起降）机型的姿态设定
	if (_vtol) {
		// Construct attitude setpoint rotation matrix. Modify the setpoints for roll
		// and pitch such that they reflect the user's intention even if a large yaw error
		// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
		// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
		// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
		// heading of the vehicle.
		// However there's also a coupling effect that causes oscillations for fast roll/pitch changes
		// at higher tilt angles, so we want to avoid using this on multicopters.
		// The effect of that can be seen with:
		// - roll/pitch into one direction, keep it fixed (at high angle)
		// - apply a fast yaw rotation
		// - look at the roll and pitch angles: they should stay pretty much the same as when not yawing

		// calculate our current yaw error
		float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw);

		// compute the vector obtained by rotating a z unit vector by the rotation
		// given by the roll and pitch commands of the user
        // zB表示初始向量，即垂直于机身的向量(0,0,1)
		Vector3f zB = {0.0f, 0.0f, 1.0f};
        
        // 使用欧拉角构造一个方向余弦矩阵（DCM），该矩阵描述了一个旋转，将一个向量从机体坐标系（body frame）旋转到一个参考坐标系（reference frame）
		Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f);
        // 将一个表示沿着z轴方向的  z_roll_pitch_sp，
        // 表示这个单位向量在机体坐标系下的方向。
        // 将Dcmf类型的 R_sp_roll_pitch旋转矩阵和Vector3f 类型的zB向量相乘，实际上是将zB向量沿着roll和pitc轴旋转，得到了一个新向量 z_roll_pitch_sp
        // 这里可以把 R_sp_roll_pitch 看成是从机体坐标系到惯性坐标系的旋转矩阵，zB 表示沿着机体坐标系 z 轴的方向，
        //			z_roll_pitch_sp 则是表示沿着机体坐标系 roll 和 pitch 方向的单位向量
		Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;

		// transform the vector into a new frame which is rotated around the z axis
		// by the current yaw error. this vector defines the desired tilt when we look
		// into the direction of the desired heading
        // 将旋转矩阵 R_sp_roll_pitch 绕着z轴 旋转一个角度 yaw_error ，得到新的向量 z_roll_pitch_sp。
        // 由于欧拉角和旋转矩阵是等价的，可以通过欧拉角构建旋转矩阵
		Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
        // 将沿着机体坐标系 roll 和 pitch 方向的单位向量 z_roll_pitch_sp 乘以旋转矩阵 R_yaw_correction
        // 就得到在当前偏航误差下的新向量，这个新向量表示期望的俯仰和滚转角度。
		z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;

		// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
		// R_tilt is computed from_euler; only true if cos(roll) not equal zero
		// -> valid if roll is not +-pi/2;
		attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1));
		attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
	}

	/* copy quaternion setpoint to attitude setpoint topic */
    // 将欧拉角转换为四元数。使用欧拉角设置飞行器的期望角度
    // 四元数可以更方便地表示飞行器的姿态，因为它们不容易受到万向锁问题的影响，并且在执行姿态控制时具有更好的数学性质
	Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
	q_sp.copyTo(attitude_setpoint.q_d);
	// 设置了期望姿态的体轴系的油门
    // 对手动控制器的纵轴输入值进行了处理，使其在0到1之间，并进行了缩放和反向操作（将输入值反向，以便使油门随着手动控制器的上升而增加）。
    // 最后将油门值设置为一个在0到-1之间的值，其中负数表示将油门放置在飞机体轴系下的“下方”，即向地面施加一个向上的力。
	attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f));
	attitude_setpoint.timestamp = hrt_absolute_time();
	//发布
	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
}

5.姿态控制

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
/*先进行roll-pitch控制-------------------------------------------------------------------------------------------------------------------*/
    // 将当前期望的姿态四元数存储在 qd 中
	Quatf qd = _attitude_setpoint_q;		// 当前飞行器的姿态 q 和 期望姿态 qd

	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch（优先考虑俯仰和滚转方向，而忽略偏航方向）
	const Vector3f e_z = q.dcm_z();			// 计算当前姿态（q）的Z轴单位向量，也就是当前飞行器的朝向向量。该向量通常被视为飞行器的“前方”。
	const Vector3f e_z_d = qd.dcm_z();		// 计算期望姿态（qd）的Z轴向量
	Quatf qd_red(e_z, e_z_d);			   // 表示从当前姿态到期望姿态的旋转
// 由于目标姿态的 $\hat{z}$ 向量（期望的推力方向）与当前姿态的 $\hat{z}$ 向量不同，所以需要旋转当前姿态使得 $\hat{z}$ 与期望的 $\hat{z}$ 方向对齐，然后在这个基础上对 roll 和 pitch 进行控制。这样做可以确保在控制 roll 和 pitch 的同时保持飞机在期望的推力方向上运动。
    
	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
		// 期望姿态和当前姿态之差比较小，直接将期望姿态转化为当前姿态
		qd_red = qd;

	} else {
		// 较大情况：将期望的姿态旋转到当前的机体坐标系中，变成了基于当前姿态的期望姿态，这样就可以通过控制 roll 和 pitch 实现对机体在推力方向上的运动控制了
		qd_red *= q;
	}
    
/*再进行Yaw控制--------------------------------------------------------------------------------------------------------------------------*/
	/* 计算当前的姿态与期望姿态的误差，并规范化误差四元数 */
    // 计算 将 当前姿态转换到期望姿态 需要 旋转的四元数（从当前姿态到期望姿态的旋转的差）
	Quatf q_mix = qd_red.inversed() * qd;	// inversed（）用于计算四元数的逆
    
    // 由于上述计算可能会出现镜像或近似镜像的情况，所以需要规范化误差四元数（即使存在数值误差也能保证旋转的正确性）
	q_mix.canonicalize();	// canonicalize（）用于将四元数规范化
    
	// catch numerical problems with the domain of acosf and asinf
    // 由于 q_mix 可能存在数值问题，超出了 acosf 和 asinf 的定义域，所以这里使用了 constrain 函数进行限制，使其在合理的范围内
	q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
	q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
    // 计算期望姿态
    /* cosf(_yaw_w * acosf(q_mix(0)))计算需要施加到期望姿态的旋转四元数中的标量部分，以确保正确的旋转方向
     * 1、q_mix(0)表示从期望姿态到当前姿态的旋转四元数中的标量部分，即旋转角度的余弦值
     * 2、acosf(q_mix(0))表示我们要计算的从期望姿态到当前姿态的旋转角度（弧度）：所以对q_mix(0)反余弦即可
     * 3、_yaw_w * acosf(q_mix(0))表示需要绕z轴施加的旋转角度，其中_yaw_w是yaw轴控制增益
    */
    /* sinf(_yaw_w * asinf(q_mix(3)))计算需要施加到期望姿态四元数的Z轴的sine值，以确保正确的旋转方向
     * 1、q_mix(3)表示从期望姿态到当前姿态的旋转四元数中Z轴的旋转角度的sin值
     * 2、反正弦
     * 3、同理也是需要绕z轴施加的旋转角度
    */
    /*综上所述，这行代码的作用是通过将从期望姿态到当前姿态的旋转与需要绕z轴施加的旋转组合在一起，得到一个期望姿态，其中包括滚转和俯仰控制，并绕z轴旋转Yaw角度，以实现期望的飞行姿态。*/ 
	qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

	// 四元数 qe ：需要施加的姿态控制误差。
	const Quatf qe = q.inversed() * qd;	// 当前姿态的逆 * 期望姿态

	// 三维向量 eq：当前姿态与期望姿态之间的误差
	const Vector3f eq = 2.f * qe.canonical().imag();	// 2 * qe的虚部（旋转向量）
    /* note1：这里使用了canonical()函数，是因为qe是四元数类型，它可以表示任意的三维旋转，但四元数也有多个等价表示，因此使用canonical()函数可以使它变为一个具有唯一性的表示。
       note2：在四元数和欧拉角之间进行转换时，欧拉角的转换公式中包含一个因子2，因此这里需要将差向量eq乘以2。具体来说，将差向量的每个分量乘以2后，可以得到欧拉角（roll, pitch, yaw）的变化量（d_roll, d_pitch, d_yaw）*/

	//三维向量 rate_setpoint： 期望的三个角速度分量
	matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); // eq（当前姿态与期望姿态之间的误差）点乘 _proportional_gain（比例控制器的增益矩阵）

	// Feed forward the yaw setpoint rate.
	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
	// such that it can be added to the rates setpoint.
    
    // _yawspeed_setpoint 前馈偏航设定点速率，q.inversed().dcm_z()是当前机体坐标系（即机体坐标系相对于惯性坐标系的旋转）的 z 轴单位向量在惯性坐标系中的表示
    // q.inversed().dcm_z() * _yawspeed_setpoint 期望绕 z 轴的角速度
	if (is_finite(_yawspeed_setpoint)) {
		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
	}

	// limit rates
	for (int i = 0; i < 3; i++) {
		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
	}

	return rate_setpoint;
}