ArduPilot开源代码之AP_AHRS_View
- 1. 源由
- 2. 框架设计
- 3. 重要例程
-
- 3.1 重要函数
- 3.2 辅助函数
-
- 3.2.1 AP_AHRS_View::get_rotation_body_to_ned
- 3.2.2 AP_AHRS_View::get_quat_body_to_ned
- 3.2.3 AP_AHRS_View::getLastPosNorthEastReset
- 3.2.4 AP_AHRS_View::getLastPosDownReset
- 3.2.5 AP_AHRS_View::earth_to_body2D
- 3.2.6 AP_AHRS_View::body_to_earth2D
- 3.2.7 AP_AHRS_View::get_rotation
- 3.2.8 AP_AHRS_View::rotate
- 3.3 帮助函数
- 3.4 日志记录
- 3.5 姿态角度
- 3.6 位置坐标
-
- 3.6.1 AP_AHRS_View::get_location
- 3.6.2 AP_AHRS_View::get_relative_position_NED_home
- 3.6.3 AP_AHRS_View::get_relative_position_NED_origin
- 3.6.4 AP_AHRS_View::get_relative_position_NE_home
- 3.6.5 AP_AHRS_View::get_relative_position_NE_origin
- 3.6.6 AP_AHRS_View::get_relative_position_D_home
- 3.6.7 AP_AHRS_View::get_relative_position_D_origin
- 3.7 速度
- 4. 总结
- 5. 参考资料
1. 源由
AP_AHRS相对来说用到更多的几何知识。目前,比较懒惰,但是AP_AHRS_View
类用另一种方式进行了展示。
在此,研读下代码。为后续进一步深入了解AHRS几何方面的计算转换提供一个基础。
2. 框架设计
AP_AHRS_View
类封装了与 AHRS 数据交互和处理的各种功能,包括访问和操作旋转矩阵、陀螺仪向量和三角值。它作为 AP_AHRS
对象的视图或专用接口,允许定制处理和表示方向数据。
2.1 公共方法
构造函数:
AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
- 使用
AP_AHRS
引用、旋转枚举和一个可选的俯仰修正度初始化类。
- 使用
update():
- 更新
AP_AHRS_View
对象的状态。
- 更新
析构函数:
virtual ~AP_AHRS_View() {}
- 空的虚析构函数,确保派生类的正确清理。
get_gyro():
const Vector3f &get_gyro(void) const;
- 返回平滑且校正后的陀螺仪向量。
get_gyro_latest():
Vector3f get_gyro_latest(void) const;
- 使用最新的 INS 数据返回最新的陀螺仪向量。
get_rotation_body_to_ned():
const Matrix3f &get_rotation_body_to_ned(void) const;
- 返回表示当前姿态的 DCM 旋转矩阵。
get_quat_body_to_ned():
void get_quat_body_to_ned(Quaternion &quat) const;
- 填充一个表示当前姿态的四元数对象。
set_pitch_trim():
void set_pitch_trim(float trim_deg);
- 应用俯仰修正。
三角值访问器:
- 访问滚转、俯仰和偏航的三角函数值的方法。
float cos_roll() const; float cos_pitch() const; float cos_yaw() const; float sin_roll() const; float sin_pitch() const; float sin_yaw() const;
包装方法:
- 这些方法包装了
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;
- 这些方法包装了
向量旋转:
- 在地球坐标系和机体坐标系之间旋转二维向量。
Vector2f earth_to_body2D(const Vector2f &ef_vector) const; Vector2f body_to_earth2D(const Vector2f &bf) const;
误差估计:
- 获取滚转/俯仰和偏航误差估计的平均值。
float get_error_rp(void) const; float get_error_yaw(void) const;
日志记录函数:
- 用于记录姿态和速率信息的函数。
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;
获取函数:
- 获取当前的旋转和俯仰修正值。
enum Rotation get_rotation(void) const; float get_pitch_trim() const;
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
内部接口,并且增加了一些辅助函数,获取:
- 姿态
- 位置
- 速度
如其名字所表述的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