ArduPilot 第9章 AP_AHRS

axzg

已于 2024-05-21 16:17:49 修改

阅读量1k

点赞数 7

文章标签：算法

于 2024-03-22 14:43:41 首次发布

本文链接：https://blog.csdn.net/zhanggeng0101/article/details/136366321

版权

参考文献
APM（三）：C++基础知识
https://blog.csdn.net/weixin_41869763/article/details/112545944

ArduPilot开源代码之AP_InertialSensor
https://blog.csdn.net/lida2003/article/details/131315176

Pixhawk之姿态解算篇（3）_源码姿态解算算法分析
https://blog.csdn.net/still8912/article/details/51189618/

ArduPilot开源代码之AP_InertialSensor_Backend
https://blog.csdn.net/lida2003/article/details/131276415

ArduPilot开源代码之AP_InertialSensor
https://blog.csdn.net/lida2003/article/details/131315176

ArduPilot开源飞控之AP_AHRS
https://blog.csdn.net/lida2003/article/details/133820796?spm=1001.2014.3001.5502

Ardupilot-ArduCopter-3.2.1+源码解读
https://wenku.baidu.com/view/0cef3b90b52acfc789ebc9bc.html?wkts=1711413770641&bdQuery=Ardupilot-ArduCopter-3.2.1+%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB

Ardupilot飞控姿态角与姿态角速度控制过程
https://blog.csdn.net/zhouxinlin2009/article/details/81195628

前言

本文就介绍AP_AHRS相关知识。
最终汇聚到数据算法处理的是AP_AHRS

一、AHRS初始化

1.init_ardupilot()

\ArduCopter\system.cpp

void Copter::init_ardupilot()
{
	...
	startup_INS_ground();
	...
}

2. startup_INS_ground

这个函数完成地面启动时需要的所有校准等
\ArduCopter\system.cpp

void Copter::startup_INS_ground()
{
	...
    ahrs.init();

    ...
    ins.init(scheduler.get_loop_rate_hz());

	...
}

3. AP_AHRS::init()

EKF算法类型选择(目前不再支持EKF1；
\libraries\AP_AHRS\AP_AHRS.cpp

void AP_AHRS::init()
{
    // EKF1 is no longer supported - handle case where it is selected
    if (_ekf_type.get() == 1) {
        AP_BoardConfig::config_error("EKF1 not available");
    }
#if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
    if (_ekf_type.get() == 2) {
        _ekf_type.set(3);
        EKF3.set_enable(true);
    }
#elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE
    if (_ekf_type.get() == 3) {
        _ekf_type.set(2);
        EKF2.set_enable(true);
    }
#endif

    last_active_ekf_type = (EKFType)_ekf_type.get();

    // init backends
    dcm.init();

#if HAL_NMEA_OUTPUT_ENABLED
    _nmea_out = AP_NMEA_Output::probe();
#endif
}

4.AP_InertialSensor::init(uint16_t loop_rate)

\libraries\AP_InertialSensor\AP_InertialSensor.cpp

void AP_InertialSensor::init(uint16_t loop_rate)
{
    // remember the sample rate
    _loop_rate = loop_rate;
    _loop_delta_t = 1.0f / loop_rate;

    // we don't allow deltat values greater than 10x the normal loop
    // time to be exposed outside of INS. Large deltat values can
    // cause divergence of state estimators
    _loop_delta_t_max = 10 * _loop_delta_t;

    if (_gyro_count == 0 && _accel_count == 0) {
        _start_backends();
    }
  ...
}

5.AP_InertialSensor::_start_backends()

\libraries\AP_InertialSensor\AP_InertialSensor.cpp

/*
 * Start all backends for gyro and accel measurements. It automatically calls
 * detect_backends() if it has not been called already.
 */
void AP_InertialSensor::_start_backends()

{
    detect_backends();

    for (uint8_t i = 0; i < _backend_count; i++) {
        _backends[i]->start();
    }

    if (_gyro_count == 0 || _accel_count == 0) {
        AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
    }

    // clear IDs for unused sensor instances
    for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
        _accel_id[i].set(0);
    }
    for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
        _gyro_id[i].set(0);
    }
}

6.AP_InertialSensor::detect_backends(void)

\libraries\AP_InertialSensor\AP_InertialSensor.cpp

/*
  detect available backends for this board
 */
void AP_InertialSensor::detect_backends(void)
{
	...
	#define ADD_BACKEND(x) do { \
        if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
            found_mask |= (1U<<probe_count); \
        } \
        probe_count++; \
    ...
    switch (AP_BoardConfig::get_board_type()) {
	...
		case AP_BoardConfig::PX4_BOARD_FMUV5:
    	case AP_BoardConfig::PX4_BOARD_FMUV6:
		// allow for either BMI055 or BMI088
	        ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this,
	                                                    hal.spi->get_device("bmi055_a"),
	                                                    hal.spi->get_device("bmi055_g"),
	                                                    ROTATION_ROLL_180_YAW_90));
	        ADD_BACKEND(AP_InertialSensor_BMI088::probe(*this,
	                                                    hal.spi->get_device("bmi055_a"),
	                                                    hal.spi->get_device("bmi055_g"),
	                                                    ROTATION_ROLL_180_YAW_90));
	        break;
	...
	}
  ...
}

7.AP_InertialSensor::_add_backend

\libraries\AP_InertialSensor\AP_InertialSensor.cpp

bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
{

    if (!backend) {
        return false;
    }
    if (_backend_count == INS_MAX_BACKENDS) {
        AP_HAL::panic("Too many INS backends");
    }
    _backends[_backend_count++] = backend;
    return true;
}

二、AHRS的update

1.Copter::fast_loop()

\ArduCopter\Copter.cpp

// Main loop - 400hz
void Copter::fast_loop()
{
	...
	立即更新INS获取当前陀螺仪数据
	ins.update();
	
	角速度PID运算的函数
	attitude_control->rate_controller_run();

	输出电机PWM值的函数
	motors_output();
	
	更新传感器并更新姿态的函数
	read_AHRS();
	...
	// run the attitude controllers
    update_flight_mode();
}

2 ins.update();

2.1 AP_InertialSensor::update

\libraries\AP_InertialSensor\AP_InertialSensor.cpp

/*
  update gyro and accel values from backends
 */
void AP_InertialSensor::update(void)
{
	...
	for (uint8_t i=0; i<_backend_count; i++) {
        _backends[i]->update();
    }
    ...
}

AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];可以看到_backends[]是个指针数组，我们找到这些指针指向哪些变量，就可以找到该变量对应的类里面的update()函数了。

class AP_InertialSensor : AP_AccelCal_Client
{
    friend class AP_InertialSensor_Backend;
...
private:
	...
	AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
	...
}

2.2 AP_InertialSensor_BMI055::update

以BMI055为例

在AP_InertialSensor.cpp找到了void AP_InertialSensor:: _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &))函数的定义。
再找到了AP_InertialSensor::_detect_backends(void)函数里进行了ADD_BACKEND(AP_InertialSensor_BMI055::probe的操作。因此_backends指向的是AP_InertialSensor_BMI055，最后，我们在AP_InertialSensor_BMI055.cpp里面找到了bool AP_InertialSensor_BMI055::update(void)的定义：（使用_publish_accel等函数更新传感器的值）

bool AP_InertialSensor_BMI055::update()
{
    update_accel(accel_instance);
    update_gyro(gyro_instance);
    return true;
}

3 attitude_control->rate_controller_run()

该函数就是进入角速度PID运算的入口。

3 .1 AC_AttitudeControl_Multi::rate_controller_run()

\libraries\AC_AttitudeControl\AC_AttitudeControl_Multi.cpp

void AC_AttitudeControl_Multi::rate_controller_run()
{
    更新油门到电机需要的值
    update_throttle_rpy_mix();

    _ang_vel_body += _sysid_ang_vel_body;
	获取最新的姿态角速度数据
    Vector3f gyro_latest = _ahrs.get_gyro_latest();

	进行内环角速度环控制
	横滚角速度PID控制器 
    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
    _motors.set_roll_ff(get_rate_roll_pid().get_ff());
	俯仰角速度PID控制器
    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
    _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
	偏航角速度PID控制器
    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);

    _sysid_ang_vel_body.zero();
    _actuator_sysid.zero();

    control_monitor_update();
}

4 Copter::read_AHRS

\ArduCopter\Copter.cpp

void Copter::read_AHRS(void)
{
    // we tell AHRS to skip INS update as we have already done it in fast_loop()
    ahrs.update(true);
}

4.1 AP_AHRS::update

\libraries\AP_AHRS\AP_AHRS.cpp

AHRS更新过程：
配置及传感数据更新；
EKF算法运算更新；

void AP_AHRS::update(bool skip_ins_update)
{
	...
	update_DCM();
	...
	if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
		#if HAL_NAVEKF2_AVAILABLE
		        update_EKF2();
		#endif
		#if HAL_NAVEKF3_AVAILABLE
		        update_EKF3();
		#endif
    } else {
		        // otherwise run EKF3 first
		#if HAL_NAVEKF3_AVAILABLE
		        update_EKF3();
		#endif
		#if HAL_NAVEKF2_AVAILABLE
		        update_EKF2();
		#endif
    }
	...
}

4.2 update_DCM()

\libraries\AP_AHRS\AP_AHRS.cpp

void AP_AHRS::update_DCM()
{
    dcm.update();
    dcm.get_results(dcm_estimates);

    // we always update the vehicle's canonical roll/pitch/yaw from
    // DCM.  In normal operation this will usually be over-written by
    // an EKF or external AHRS.  This is long-held behaviour, but this
    // really shouldn't be doing this.

    // if (active_EKF_type() == EKFType::NONE) {
        copy_estimates_from_backend_estimates(dcm_estimates);
    // }
}

4.3 AP_AHRS_DCM::update()

\libraries\AP_AHRS\AP_AHRS_DCM.cpp

// run a full DCM update round
void
AP_AHRS_DCM::update()
{
    AP_InertialSensor &_ins = AP::ins();

    // ask the IMU how much time this sensor reading represents
    const float delta_t = _ins.get_delta_time();

    // if the update call took more than 0.2 seconds then discard it,
    // otherwise we may move too far. This happens when arming motors
    // in ArduCopter
    if (delta_t > 0.2f) {
        memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum));
        _ra_deltat = 0;
        return;
    }

    就是使用刚刚测量出来的陀螺仪值、以及上个周期对陀螺仪的补偿值进行角度更新的函数
    使用陀螺仪更新四元素矩阵
    // Integrate the DCM matrix using gyro inputs
    matrix_update(delta_t);
    
	用来做余弦矩阵的归一化
	在normalize函数里调用->AP_AHRS_DCM::reset(true)-> _ins.update();
    // Normalize the DCM matrix
    normalize();
    
	使用电子罗盘、加速度计、和GPS来计算角度误差，并更新_omega（_omega会在下一个周期的matrix_update(delta_t)中被使用）
	使用加速度计和罗盘与该次计算的四元素矩阵做差，修正下一次的陀螺仪输出
    // Perform drift correction
    drift_correction(delta_t);

	在check_matrix函数调用->AP_AHRS_DCM::reset(true)-> _ins.update();
    // paranoid check for bad values in the DCM matrix
    check_matrix();

    // calculate the euler angles and DCM matrix which will be used
    // for high level navigation control. Apply trim such that a
    // positive trim value results in a positive vehicle rotation
    // about that axis (ie a negative offset)
    _body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
    转换成欧拉角
    _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);

    // pre-calculate some trig for CPU purposes:
    _cos_yaw = cosf(yaw);
    _sin_yaw = sinf(yaw);

    backup_attitude();

    // remember the last origin for fallback support
    IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
}



## 8.AP_AHRS::update_EKF2
\libraries\AP_AHRS\AP_AHRS.cpp

```cpp
void AP_AHRS::update_EKF2(void)
{
	...
	 EKF2.UpdateFilter();
	 ...
}

4.4.NavEKF2::UpdateFilter

\libraries\AP_NavEKF2\AP_NavEKF2.cpp

void NavEKF2::UpdateFilter(void)
{
	...
	for (uint8_t i=0; i<num_cores; i++) {
		...
		core[i].UpdateFilter(statePredictEnabled[i]);
		...
	}
	...
}

4.5.NavEKF2_core::UpdateFilter

\libraries\AP_NavEKF2\AP_NavEKF2_core.cpp

void NavEKF2_core::UpdateFilter(bool predict)
{
	...
	readIMUData();
   	if (runUpdates) {
		// Update the filter status
        updateFilterStatus();
        ...
    }
    ...
}

5 update_flight_mode()

5.1 Copter::update_flight_mode()

\ArduCopter\mode.cpp

void Copter::update_flight_mode()
{
    surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used

    flightmode->run();
}

5.2 ModeStabilize::run()

\ArduCopter\modestabilize.cpp
接下来是update_flight_mode()。前面都比较简单我们直接跳进Copter::stabilize_run()去分析。stabilize_run()这个函数大部分工作其实都是在更新和计算目标值_rate_bf_target。
~~其中，update_simple_mode()是读取遥控器的信号，~~ 该函数里面的channel_roll->control_in是通过如下获取的：Copter::rc_loop()（10ms执行一次）里面的Copter::read_radio()里面的RC_Channel::set_pwm_all的rc_ch[i]->set_pwm(rc_ch[i]->read())，RC_Channel::set_pwm(int16_t pwm)里面的control_in = pwm_to_range()或control_in = pwm_to_angle()，pwm_to_range()里面的pwm_to_range_dz(_dead_zone)，最终返回return (_low + ((int32_t)(_high - _low) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low))。（例如对于油门，往上拨动到最多，返回1000，往下拨到最小，返回_low）

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void ModeStabilize::run()
{
    // apply simple mode transform to pilot inputs
    update_simple_mode();

    // convert pilot input to lean angles
    float target_roll, target_pitch;
    get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);

    // get pilot's desired yaw rate
    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());

    if (!motors->armed()) {
        // Motors should be Stopped
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
    } else if (copter.ap.throttle_zero) {
        // Attempting to Land
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
    } else {
        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
    }

    switch (motors->get_spool_state()) {
    case AP_Motors::SpoolState::SHUT_DOWN:
        // Motors Stopped
        attitude_control->reset_yaw_target_and_rate();
        attitude_control->reset_rate_controller_I_terms();
        break;

    case AP_Motors::SpoolState::GROUND_IDLE:
        // Landed
        attitude_control->reset_yaw_target_and_rate();
        attitude_control->reset_rate_controller_I_terms_smoothly();
        break;

    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
        // clear landing flag above zero throttle
        if (!motors->limit.throttle_lower) {
            set_land_complete(false);
        }
        break;

    case AP_Motors::SpoolState::SPOOLING_UP:
    case AP_Motors::SpoolState::SPOOLING_DOWN:
        // do nothing
        break;
    }

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

    // output pilot's throttle
    attitude_control->set_throttle_out(get_pilot_desired_throttle(),
                                       true,
                                       g.throttle_filt);
}

5.2.1 update_simple_mode()

接着，在update_simple_mode()中根据simple_mode对control_in进行进一步换算：
a. 在默认情况下，simple_mode为0，不作任何处理，一直以飞机的朝向来定roll和pitch方向；
b. 当simple_mode为1时，则以刚起飞时飞行器面向的方向为参考计算roll和pitch期望值。例如刚起飞的时候飞行器朝向正北，那么起飞以后，无论飞行器跑到哪里，朝向改到哪里（朝向改到东西南都一样），我们调整遥控器的roll摇杆就是往东或者西方向走，调整遥控器的pitch摇杆就是往南或者北方向走。这种模式就是方便操控者一直面向一个方向，不需要考虑飞行器本身的方向。
c. 当simple_mode大于1时，则以飞行器目前的坐标位置与起飞坐标位置的连线来计算roll和pitch期望值。例如某一时刻飞行器飞到起飞坐标的北边，那么我们调整遥控器的roll摇杆就是往东或者西方向走，调整遥控器的pitch摇杆就是往南或者北方向走；过一会飞行器飞到起飞坐标的西边，那么我们调整遥控器的roll摇杆就是往南或者北方向走，调整遥控器的pitch摇杆就是往东或者西方向走。这种模式方便操控者不断地面向飞机来操作。
具体实现就是作正弦余弦换算，弄明白上面的目的，推导起来就比较简单：

// update_simple_mode - rotates pilot input if we are in simple mode
void Copter::update_simple_mode(void)
{
    float rollx, pitchx;

    // exit immediately if no new radio frame or not in simple mode
    if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
        return;
    }

    // mark radio frame as consumed
    ap.new_radio_frame = false;

    if (simple_mode == SimpleMode::SIMPLE) {
        // rotate roll, pitch input by -initial simple heading (i.e. north facing)
        rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
        pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
    }else{
        // rotate roll, pitch input by -super simple heading (reverse of heading to home)
        rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
        pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
    }

    // rotate roll, pitch input from north facing to vehicle's perspective
    channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
    channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}

5.2.2 get_pilot_desired_lean_angles

回到Copter::stabilize_run()再继续往下看，get_pilot_desired_lean_angles()是把遥控器的刻度转换为角度的刻度； target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in)是对遥控器yaw的输入进行换算；pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in)是对遥控器throttle的输入进行换算（中间设为死区）。
这个代码主要把横滚输入，俯仰输入量转换成目标角度需要的范围，也就是+4500，-4500

\ArduCopter\mode.cpp

void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
    // throttle failsafe check
    if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
        roll_out = 0;
        pitch_out = 0;
        return;
    }
    表示遥控器的输入量
    // fetch roll and pitch inputs
    roll_out = channel_roll->get_control_in();
    pitch_out = channel_pitch->get_control_in();

    // limit max lean angle
    angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);

    // scale roll and pitch inputs to ANGLE_MAX parameter range
    float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
    roll_out *= scaler;
    pitch_out *= scaler;

    // do circular limit
    float total_in = norm(pitch_out, roll_out);
    if (total_in > angle_limit) {
        float ratio = angle_limit / total_in;
        roll_out *= ratio;
        pitch_out *= ratio;
    }

    // do lateral tilt to euler roll conversion
    roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));

    // roll_out and pitch_out are returned
}

5.2.3 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw

\ArduCopter\mode.cpp
最重要的姿态控制器

void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{

    转换得到需要的目标弧度值 
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);//缩小100倍–[-45,45]

    float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);//缩小100倍

    float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);//缩小100倍

	
	
	获取当前无人机实际的姿态角 获取欧拉角 横滚角 俯仰角 偏航角
    // calculate the attitude target euler angles
    _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);

    补偿处理（将在多旋翼上返回零）
    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle += get_roll_trim_rad();
	
	姿态pid运算
    if (_rate_bf_ff_enabled) {
        将横滚，俯仰和偏航加速度极限转换为欧拉轴
        // translate the roll pitch and yaw acceleration limits to the euler axis
        const Vector3f euler_accel = euler_accel_limit(_euler_angle_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});

        pid运算
        _euler_rate_target.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), _input_tc, euler_accel.x, _euler_rate_target.x, _dt);
        _euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt);
        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        _euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt);

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        将实际的姿态的欧拉角导数，转化到机体坐标系中速度矢量，作为前馈速度矢量
        euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
        // Limit the angular velocity
        ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
        // Convert body-frame angular velocity into euler angle derivative of desired attitude
        ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
    } else {
	// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
        _euler_angle_target.x = euler_roll_angle;
        _euler_angle_target.y = euler_pitch_angle;
        _euler_angle_target.z += euler_yaw_rate * _dt;
        // Compute quaternion target attitude
        _attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);

        // Set rate feedforward requests to zero
        _euler_rate_target.zero();
        _ang_vel_target.zero();
    }
    调用四元数姿态控制器
    // Call quaternion attitude controller
    attitude_controller_run_quat();
}