ArduPilot开源代码之AP_AHRS_View

1. 源由

AP_AHRS相对来说用到更多的几何知识。目前，比较懒惰，但是AP_AHRS_View 类用另一种方式进行了展示。

在此，研读下代码。为后续进一步深入了解AHRS几何方面的计算转换提供一个基础。

2. 框架设计

AP_AHRS_View 类封装了与 AHRS 数据交互和处理的各种功能，包括访问和操作旋转矩阵、陀螺仪向量和三角值。它作为 AP_AHRS 对象的视图或专用接口，允许定制处理和表示方向数据。

2.1 公共方法

1. 构造函数：

   AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
   

   • 使用 AP_AHRS 引用、旋转枚举和一个可选的俯仰修正度初始化类。

2. update()：

   • 更新 AP_AHRS_View 对象的状态。

3. 析构函数：

   virtual ~AP_AHRS_View() {}
   

   • 空的虚析构函数，确保派生类的正确清理。

4. get_gyro()：

   const Vector3f &get_gyro(void) const;
   

   • 返回平滑且校正后的陀螺仪向量。

5. get_gyro_latest()：

   Vector3f get_gyro_latest(void) const;
   

   • 使用最新的 INS 数据返回最新的陀螺仪向量。

6. get_rotation_body_to_ned()：

   const Matrix3f &get_rotation_body_to_ned(void) const;
   

   • 返回表示当前姿态的 DCM 旋转矩阵。

7. get_quat_body_to_ned()：

   void get_quat_body_to_ned(Quaternion &quat) const;
   

   • 填充一个表示当前姿态的四元数对象。

8. set_pitch_trim()：

   void set_pitch_trim(float trim_deg);
   

   • 应用俯仰修正。

9. 三角值访问器：

   • 访问滚转、俯仰和偏航的三角函数值的方法。

   float cos_roll() const;
   float cos_pitch() const;
   float cos_yaw() const;
   float sin_roll() const;
   float sin_pitch() const;
   float sin_yaw() const;
   

10. 包装方法：

    • 这些方法包装了 ahrs 函数，提供直接访问。

    bool get_location(Location &loc) const WARN_IF_UNUSED;
    bool wind_estimate(Vector3f &wind);
    bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED;
    bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED;
    float get_EAS2TAS(void) const;
    Vector2f groundspeed_vector(void);
    bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;
    bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED;
    bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED;
    bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED;
    bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED;
    void get_relative_position_D_home(float &posD) const;
    bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED;
    float groundspeed(void);
    const Vector3f &get_accel_ef(void) const;
    uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED;
    uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED;
    

11. 向量旋转：

    • 在地球坐标系和机体坐标系之间旋转二维向量。

    Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
    Vector2f body_to_earth2D(const Vector2f &bf) const;
    

12. 误差估计：

    • 获取滚转/俯仰和偏航误差估计的平均值。

    float get_error_rp(void) const;
    float get_error_yaw(void) const;
    

13. 日志记录函数：

    • 用于记录姿态和速率信息的函数。

    void Write_AttitudeView(const Vector3f &targets) const;
    void Write_Rate(const class AP_Motors &motors, const class AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) const;
    

14. 获取函数：

    • 获取当前的旋转和俯仰修正值。

    enum Rotation get_rotation(void) const;
    float get_pitch_trim() const;
    

15. rotate()：

    • 将向量从 AHRS 参考系旋转到 AHRS 视图参考系。

    void rotate(Vector3f &vec) const;
    

2.2 私有成员

• Rotation：存储旋转枚举。
• AP_AHRS 引用：对 AP_AHRS 对象的引用。
• 矩阵：rot_view、rot_view_T、rot_body_to_ned 用于处理旋转。
• 向量：gyro 用于存储陀螺仪数据。
• 三角值：一个结构体，用于存储滚转、俯仰和偏航的余弦和正弦值。
• 角度：y_angle 和 _pitch_trim_deg 用于处理俯仰修正。

2.3 动态过程

动态过程可以理解为代码动态执行中使用到的维护过程，通常使用：进程、线程、定时、中断等方式。

2.3.1 初始化

AP_Vehicle::setup
 └──> Copter::init_ardupilot
     └──> Copter::allocate_motors
         └──> AP_AHRS::create_view  //ahrs_view
             └──> _view = new AP_AHRS_View(*this, rotation, pitch_trim_deg);

2.3.2 定时轮训

FAST_TASK(read_AHRS)
 └──> Copter::read_AHRS
     └──> AP_AHRS::update
         └──> AP_AHRS_View::update

3. 重要例程

3.1 重要函数

3.1.1 AP_AHRS_View::AP_AHRS_View

AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg)
|
|-- 初始化列表
|   |-- rotation(_rotation)
|   |-- ahrs(_ahrs)
|
|-- switch (rotation)  // 根据不同的旋转角度设置 y_angle
|   |-- case ROTATION_NONE: // 无旋转
|   |   |-- y_angle = 0
|   |
|   |-- case ROTATION_PITCH_90: // 俯仰旋转90度
|   |   |-- y_angle = 90
|   |
|   |-- case ROTATION_PITCH_270: // 俯仰旋转270度
|   |   |-- y_angle = 270
|   |
|   |-- default:  // 其他未支持的旋转角度，触发错误
|       |-- AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation)
|
|-- _pitch_trim_deg = pitch_trim_deg
|   // 添加俯仰微调角度
|-- rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0)
|-- rot_view_T = rot_view
|-- rot_view_T.transpose()
|
|-- update()

3.1.2 AP_AHRS_View::update

AP_AHRS_View::update()
|
|-- 获取机体到北东地坐标系的旋转矩阵
|   rot_body_to_ned = ahrs.get_rotation_body_to_ned();
|
|-- 获取陀螺仪数据
|   gyro = ahrs.get_gyro();
|
|-- 如果俯仰角度加上俯仰修正角度不为零
|   if (!is_zero(y_angle + _pitch_trim_deg)) {
|   |
|   |-- 将旋转矩阵乘以视角旋转矩阵
|   |   rot_body_to_ned = rot_body_to_ned * rot_view_T;
|   |
|   |-- 将陀螺仪数据乘以视角旋转矩阵
|       gyro = rot_view * gyro;
|   }
|
|-- 将旋转矩阵转换为欧拉角（滚转、俯仰和偏航）
|   rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
|
|-- 将滚转、俯仰和偏航角度转换为度数并放大100倍
|   roll_sensor  = degrees(roll) * 100;
|   pitch_sensor = degrees(pitch) * 100;
|   yaw_sensor   = degrees(yaw) * 100;
|
|-- 如果偏航角度为负数，将其调整为正数
|   if (yaw_sensor < 0) {
|       yaw_sensor += 36000;
|   }
|
|-- 计算旋转矩阵的三角函数值
|   ahrs.calc_trig(rot_body_to_ned,
|                  trig.cos_roll, trig.cos_pitch, trig.cos_yaw,
|                  trig.sin_roll, trig.sin_pitch, trig.sin_yaw);

3.2 辅助函数

3.2.1 AP_AHRS_View::get_rotation_body_to_ned

// 返回一个DCM旋转矩阵，表示我们在这个视图中的当前姿态
const Matrix3f &get_rotation_body_to_ned(void) const {
    return rot_body_to_ned;
}

3.2.2 AP_AHRS_View::get_quat_body_to_ned

// 返回一个四元数，表示我们在该视角下的当前姿态
void get_quat_body_to_ned(Quaternion &quat) const {
    quat.from_rotation_matrix(rot_body_to_ned);
}

3.2.3 AP_AHRS_View::getLastPosNorthEastReset

// 获取上次北东重置位置
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
    return ahrs.getLastPosNorthEastReset(pos);
}

3.2.4 AP_AHRS_View::getLastPosDownReset

// 获取最后一次位置向下重置的时间戳并返回
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
    return ahrs.getLastPosDownReset(posDelta);
}

3.2.5 AP_AHRS_View::earth_to_body2D

// 将一个二维向量从地球坐标系旋转到机体坐标系
Vector2f AP_AHRS_View::earth_to_body2D(const Vector2f &ef) const
{
    return Vector2f(ef.x * trig.cos_yaw + ef.y * trig.sin_yaw,
                    -ef.x * trig.sin_yaw + ef.y * trig.cos_yaw);
}

3.2.6 AP_AHRS_View::body_to_earth2D

// 将二维向量从地球坐标系旋转到机体坐标系
Vector2f AP_AHRS_View::body_to_earth2D(const Vector2f &bf) const
{
    return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
                    bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
}

3.2.7 AP_AHRS_View::get_rotation

// 获取当前旋转角度
// 注意：这可能不是实际使用的旋转角度，请查看 _pitch_trim_deg
enum Rotation get_rotation(void) const {
    return rotation;
}

3.2.8 AP_AHRS_View::rotate

// 将向量从AHRS参考框架旋转到AHRS视图参考框架
void AP_AHRS_View::rotate(Vector3f &vec) const
{
    vec = rot_view * vec;  // 使用rot_view矩阵对向量进行旋转
}

3.3 帮助函数

3.3.1 AP_AHRS_View::

    float cos_roll() const {
        return trig.cos_roll;
    }

3.3.2 AP_AHRS_View::

    float cos_pitch() const {
        return trig.cos_pitch;
    }

3.3.3 AP_AHRS_View::

    float cos_yaw() const {
        return trig.cos_yaw;
    }

3.3.4 AP_AHRS_View::

    float sin_roll() const {
        return trig.sin_roll;
    }

3.3.5 AP_AHRS_View::

    float sin_pitch() const {
        return trig.sin_pitch;
    }

3.3.6 AP_AHRS_View::

    float sin_yaw() const {
        return trig.sin_yaw;
    }

3.4 日志记录

3.4.1 AP_AHRS_View::Write_AttitudeView

// Write an attitude view packet
void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
{
    const struct log_Attitude pkt{
        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
        time_us         : AP_HAL::micros64(),
        control_roll    : (int16_t)targets.x,
        roll            : (int16_t)roll_sensor,
        control_pitch   : (int16_t)targets.y,
        pitch           : (int16_t)pitch_sensor,
        control_yaw     : (uint16_t)wrap_360_cd(targets.z),
        yaw             : (uint16_t)wrap_360_cd(yaw_sensor),
        error_rp        : (uint16_t)(get_error_rp() * 100),
        error_yaw       : (uint16_t)(get_error_yaw() * 100),
        active          : uint8_t(AP::ahrs().active_EKF_type()),
    };
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

3.4.2 AP_AHRS_View::Write_Rate

// Write a rate packet
void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
                                const AC_PosControl &pos_control) const
{
    const Vector3f &rate_targets = attitude_control.rate_bf_targets();
    const Vector3f &accel_target = pos_control.get_accel_target_cmss();
    const auto timeus = AP_HAL::micros64();
    const struct log_Rate pkt_rate{
        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
        time_us         : timeus,
        control_roll    : degrees(rate_targets.x),
        roll            : degrees(get_gyro().x),
        roll_out        : motors.get_roll()+motors.get_roll_ff(),
        control_pitch   : degrees(rate_targets.y),
        pitch           : degrees(get_gyro().y),
        pitch_out       : motors.get_pitch()+motors.get_pitch_ff(),
        control_yaw     : degrees(rate_targets.z),
        yaw             : degrees(get_gyro().z),
        yaw_out         : motors.get_yaw()+motors.get_yaw_ff(),
        control_accel   : (float)accel_target.z,
        accel           : (float)(-(get_accel_ef().z + GRAVITY_MSS) * 100.0f),
        accel_out       : motors.get_throttle(),
        throttle_slew   : motors.get_throttle_slew_rate()
    };
    AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));

    /*
      log P/PD gain scale if not == 1.0
     */
    const Vector3f &scale = attitude_control.get_last_angle_P_scale();
    const Vector3f &pd_scale = attitude_control.get_PD_scale_logging();
    if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) {
        const struct log_ATSC pkt_ATSC {
            LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
            time_us  : timeus,
            scaleP_x : scale.x,
            scaleP_y : scale.y,
            scaleP_z : scale.z,
            scalePD_x : pd_scale.x,
            scalePD_y : pd_scale.y,
            scalePD_z : pd_scale.z,
        };
        AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
    }
}

3.5 姿态角度

3.5.1 AP_AHRS_View::set_pitch_trim

// 应用俯仰修正角度
void AP_AHRS_View::set_pitch_trim(float trim_deg) {
    _pitch_trim_deg = trim_deg;  // 设置俯仰修正角度

    // 根据当前视角角度（y_angle）和修正角度（_pitch_trim_deg）计算旋转矩阵
    rot_view.from_euler(0, radians(wrap_360(y_angle + _pitch_trim_deg)), 0);

    rot_view_T = rot_view;  // 复制旋转矩阵
    rot_view_T.transpose(); // 转置旋转矩阵
};

3.5.2 AP_AHRS_View::get_pitch_trim

// 获取俯仰修正角度（度）
float get_pitch_trim() const { return _pitch_trim_deg; }

3.5.3 AP_AHRS_View::get_error_rp

// 返回自上次调用以来滚转/俯仰误差估计的平均大小
float get_error_rp(void) const {
    return ahrs.get_error_rp();  // 调用ahrs对象的get_error_rp方法，返回滚转/俯仰误差的平均大小
}

3.5.4 AP_AHRS_View::get_error_yaw

// 返回航向误差估计的平均大小，自上次调用以来
float get_error_yaw(void) const {
    return ahrs.get_error_yaw(); // 调用ahrs对象的方法获取航向误差
}

3.5.5 AP_AHRS_View::get_gyro

// 返回一个平滑和校正后的陀螺仪向量
const Vector3f &get_gyro(void) const {
    return gyro;
}

3.5.6 AP_AHRS_View::get_gyro_latest

// 返回一个经过平滑和校正的陀螺仪向量，使用最新的惯性导航系统数据
Vector3f AP_AHRS_View::get_gyro_latest(void) const {
    // rot_view 是一个旋转矩阵，用于视图旋转
    // ahrs.get_gyro_latest() 返回最新的惯性导航系统（AHRS）中的陀螺仪数据
    return rot_view * ahrs.get_gyro_latest();
}

3.6 位置坐标

3.6.1 AP_AHRS_View::get_location

// 获取位置信息的函数，返回一个布尔值表示是否成功获取位置信息
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_location(Location &loc) const WARN_IF_UNUSED {
    // 调用 ahrs 对象的 get_location 方法，将位置信息存储到 loc 中，并返回结果
    return ahrs.get_location(loc);
}

3.6.2 AP_AHRS_View::get_relative_position_NED_home

// 获取相对于“origin”（起始位置）的D相对位置
// 参数：Vector3f &vec
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
	return ahrs.get_relative_position_NED_home(vec);
}

3.6.3 AP_AHRS_View::get_relative_position_NED_origin

// 获取相对于“origin”（起始位置）的D相对位置
// 参数：Vector3f &vec
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
	return ahrs.get_relative_position_NED_origin(vec);
}

3.6.4 AP_AHRS_View::get_relative_position_NE_home

// 获取相对于“home”（起始位置）的D相对位置
// 参数：Vector2f &vecNE
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
    return ahrs.get_relative_position_NE_home(vecNE);
}

3.6.5 AP_AHRS_View::get_relative_position_NE_origin

// 获取相对于“origin”（起始位置）的D相对位置
// 参数：Vector2f &vecNE
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED {
    return ahrs.get_relative_position_NE_origin(vecNE);
}

3.6.6 AP_AHRS_View::get_relative_position_D_home

// 获取相对于“home”（起始位置）的D相对位置
// 参数：float &posD
void get_relative_position_D_home(float &posD) const {
    // 调用 ahrs 对象的 get_relative_position_D_home 方法来获取相对位置
    ahrs.get_relative_position_D_home(posD);
}

3.6.7 AP_AHRS_View::get_relative_position_D_origin

// 获取相对于“origin”（起始位置）的D相对位置
// 参数：float &posD
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED {
    // 调用 ahrs 对象的 get_relative_position_D_origin 方法，并将结果返回
    return ahrs.get_relative_position_D_origin(posD);
}

3.7 速度

3.7.1 AP_AHRS_View::wind_estimate

// 获取类型为Vector3f的风速
// 参数：Vector3f &wind
bool wind_estimate(Vector3f &wind) {
    // 调用 ahrs 对象的 wind_estimate 方法，传入 wind 引用参数
    return ahrs.wind_estimate(wind);
}

3.7.2 AP_AHRS_View::airspeed_estimate

// 定义一个名为 airspeed_estimate 的方法，接受一个引用参数 airspeed_ret，返回类型为布尔值。
// 方法被声明为 const，表示该方法不会修改类的成员变量。
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED {
    // 调用 ahrs 对象的 airspeed_estimate 方法，并将结果返回。
    return ahrs.airspeed_estimate(airspeed_ret);
}

3.7.3 AP_AHRS_View::airspeed_estimate_true

// 定义了一个名为 airspeed_estimate_true 的布尔类型方法
// 该方法接收一个浮点数引用参数 airspeed_ret，并返回一个布尔值
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
    // 调用 ahrs 对象的 airspeed_estimate_true 方法，并将结果返回
    return ahrs.airspeed_estimate_true(airspeed_ret);
}

3.7.4 AP_AHRS_View::get_EAS2TAS

    float get_EAS2TAS(void) const {
        return ahrs.get_EAS2TAS();
    }

3.7.5 AP_AHRS_View::groundspeed_vector

// 获取类型为Vector2f的地面速度
Vector2f groundspeed_vector(void) {
    // 调用ahrs对象的groundspeed_vector方法，并返回其结果
    return ahrs.groundspeed_vector();
}

3.7.6 AP_AHRS_View::get_velocity_NED

// 获取东北地坐标系（NED）中的速度
// 参数：Vector3f &vec
// 备注：WARN_IF_UNUSED 如果该函数未被使用，会给出警告
bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
    return ahrs.get_velocity_NED(vec);
}

3.7.7 AP_AHRS_View::groundspeed

// 获取地面速度
float groundSpeed(void) {
    // 调用 ahrs 对象的 groundSpeed() 方法，并返回其结果
    return ahrs.groundspeed();
}

3.7.8 AP_AHRS_View::get_accel_ef

// 获取加速度（以地球坐标系表示）
const Vector3f &get_accel_ef(void) const {
    // 调用并返回AHRS（姿态航向参考系统）对象的get_accel_ef方法的结果
    return ahrs.get_accel_ef();
}

4. 总结

综上所述，AP_AHRS_View 类调用了大量AP_AHRS内部接口，并且增加了一些辅助函数，获取：

1. 姿态
2. 位置
3. 速度

如其名字所表述的AP_AHRS_View = AP_AHRS + another view。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源飞控之AP_AHRS