ArduPilot开源代码之AP_NavEKF_Source
- 1. 源由
- 2. 框架设计
- 3. 重要例程
- 3.1 init
- 3.2 getPosXYSource
- 3.3 getPosZSource
- 3.4 setPosVelYawSourceSet
- 3.5 getPosVelYawSourceSet
- 3.6 getVelXYSource
- 3.7 getVelZSource
- 3.8 useVelXYSource
- 3.9 useVelZSource
- 3.10 haveVelZSource
- 3.11 getYawSource
- 3.12 align_inactive_sources
- 3.13 usingGPS
- 3.14 configured
- 3.15 mark_configured
- 3.16 pre_arm_check
- 3.17 ext_nav_enabled
- 3.18 gps_yaw_enabled
- 3.19 wheel_encoder_enabled
- 3.20 get_active_source_set
- 4. 总结
- 5. 参考资料
1. 源由
AP_NavEKF_Source 类提供了一个管理各种导航来源和配置的框架。
它支持不同类型数据（位置、速度、航向）的多个来源集，并包含初始化、配置和检查这些来源状态的方法。
2. 框架设计
2.1 构造函数
AP_NavEKF_Source(): 初始化对象。
AP_NavEKF_Source::AP_NavEKF_Source()
{
AP_Param::setup_object_defaults(this, var_info);
}
2.2 枚举
SourceXY: 定义可能的 XY 位置数据来源（例如 GPS、信标等）。SourceZ: 定义可能的 Z（高度）位置数据来源（例如气压计、测距仪等）。SourceYaw: 定义可能的航向数据来源（例如罗盘、GPS 等）。SourceOptions: 配置各种选项的标志（例如融合所有速度、对齐外部导航位置等）。
enum class SourceXY : uint8_t {
NONE = 0,
// BARO = 1 (not applicable)
// RANGEFINDER = 2 (not applicable)
GPS = 3,
BEACON = 4,
OPTFLOW = 5,
EXTNAV = 6,
WHEEL_ENCODER = 7
};
enum class SourceZ : uint8_t {
NONE = 0,
BARO = 1,
RANGEFINDER = 2,
GPS = 3,
BEACON = 4,
// OPTFLOW = 5 (not applicable, optical flow can be used for terrain alt but not relative or absolute alt)
EXTNAV = 6
// WHEEL_ENCODER = 7 (not applicable)
};
enum class SourceYaw : uint8_t {
NONE = 0,
COMPASS = 1,
GPS = 2,
GPS_COMPASS_FALLBACK = 3,
EXTNAV = 6,
GSF = 8
};
// enum for OPTIONS parameter
enum class SourceOptions {
FUSE_ALL_VELOCITIES = (1 << 0), // fuse all velocities configured in source sets
ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1) // align position of inactive sources to ahrs when using optical flow
};
2.3 方法
void init(): 初始化对象。SourceXY getPosXYSource() const: 返回当前的 XY 位置来源。SourceZ getPosZSource() const: 返回当前的 Z 位置来源。void setPosVelYawSourceSet(uint8_t source_set_idx): 根据索引设置位置、速度和航向来源。uint8_t getPosVelYawSourceSet() const: 返回当前激活的来源集的索引。SourceXY getVelXYSource() const: 返回当前的 XY 速度来源。SourceZ getVelZSource() const: 返回当前的 Z 速度来源。bool useVelXYSource(SourceXY velxy_source) const: 检查给定的 XY 速度来源是否被使用。bool useVelZSource(SourceZ velz_source) const: 检查给定的 Z 速度来源是否被使用。bool haveVelZSource() const: 如果配置了 Z 速度来源，则返回 true。SourceYaw getYawSource() const: 返回当前的航向来源。void align_inactive_sources(): 将非激活来源的位置对齐到姿态航向参考系统（AHRS）。bool usingGPS() const: 如果任何来源使用 GPS，则返回 true。bool configured(): 检查参数是否已配置。void mark_configured(): 标记参数为已配置。bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const: 执行预备检查。bool ext_nav_enabled(void) const: 检查是否在任何来源中启用了外部导航。bool gps_yaw_enabled(void) const: 检查是否在任何来源中启用了 GPS 航向。bool wheel_encoder_enabled(void) const: 检查是否在任何来源中启用了车轮编码器。uint8_t get_active_source_set() const: 返回当前激活的来源集的索引。
2.4 静态成员
static const struct AP_Param::GroupInfo var_info[];: 可能包含用于配置的参数元数据。
3. 重要例程
3.1 init
【未定义，未使用】初始化对象
3.2 getPosXYSource
返回当前的 XY 位置来源
SourceXY getPosXYSource() const { return _source_set[active_source_set].posxy; }
3.3 getPosZSource
返回当前的 Z 位置来源
// get pos Z source
AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource() const
{
#ifdef HAL_BARO_ALLOW_INIT_NO_BARO
// check for special case of missing baro
if ((_source_set[active_source_set].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
return SourceZ::NONE;
}
#endif
return _source_set[active_source_set].posz;
}
3.4 setPosVelYawSourceSet
根据索引设置位置、速度和航向来源
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
{
// sanity check source idx
if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
active_source_set = source_set_idx;
#if HAL_LOGGING_ENABLED
static const LogEvent evt[AP_NAKEKF_SOURCE_SET_MAX] {
LogEvent::EK3_SOURCES_SET_TO_PRIMARY,
LogEvent::EK3_SOURCES_SET_TO_SECONDARY,
LogEvent::EK3_SOURCES_SET_TO_TERTIARY,
};
AP::logger().Write_Event(evt[active_source_set]);
#endif
}
}
3.5 getPosVelYawSourceSet
返回当前激活的来源集的索引
uint8_t getPosVelYawSourceSet() const { return active_source_set; }
3.6 getVelXYSource
返回当前的 XY 速度来源
SourceXY getVelXYSource() const { return _source_set[active_source_set].velxy; }
3.7 getVelZSource
返回当前的 Z 速度来源
SourceZ getVelZSource() const { return _source_set[active_source_set].velz; }
3.8 useVelXYSource
检查给定的 XY 速度来源是否被使用
// true/false of whether velocity source should be used
bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
{
if (velxy_source == _source_set[active_source_set].velxy) {
return true;
}
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].velxy == velxy_source) {
return true;
}
}
}
// if we got this far source should not be used
return false;
}
3.9 useVelZSource
检查给定的 Z 速度来源是否被使用
bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
{
if (velz_source == _source_set[active_source_set].velz) {
return true;
}
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].velz == velz_source) {
return true;
}
}
}
// if we got this far source should not be used
return false;
}
3.10 haveVelZSource
如果配置了 Z 速度来源，则返回 true
// true if a velocity source is configured
bool AP_NavEKF_Source::haveVelZSource() const
{
if (_source_set[active_source_set].velz != SourceZ::NONE) {
return true;
}
// check for fuse all velocities
if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].velz != SourceZ::NONE) {
return true;
}
}
}
// if we got this far no velocity z source has been configured
return false;
}
3.11 getYawSource
返回当前的航向来源
// get yaw source
AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource() const
{
// check for special case of disabled compasses
if ((_source_set[active_source_set].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
return SourceYaw::NONE;
}
return _source_set[active_source_set].yaw;
}
3.12 align_inactive_sources
将非激活来源的位置对齐到姿态航向参考系统（AHRS）
// align position of inactive sources to ahrs
void AP_NavEKF_Source::align_inactive_sources()
{
// align visual odometry
#if HAL_VISUALODOM_ENABLED
auto *visual_odom = AP::dal().visualodom();
if (!visual_odom || !visual_odom->enabled()) {
return;
}
// consider aligning ExtNav XY position:
bool align_posxy = false;
if ((getPosXYSource() == SourceXY::GPS) ||
(getPosXYSource() == SourceXY::BEACON) ||
((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
// align ExtNav position if active source is GPS, Beacon or (optionally) Optflow
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].posxy == SourceXY::EXTNAV) {
// ExtNav could potentially be used, so align it
align_posxy = true;
break;
}
}
}
// consider aligning Z position:
bool align_posz = false;
if ((getPosZSource() == SourceZ::BARO) ||
(getPosZSource() == SourceZ::RANGEFINDER) ||
(getPosZSource() == SourceZ::GPS) ||
(getPosZSource() == SourceZ::BEACON)) {
// ExtNav is not the active source; we do not want to align active source!
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (_source_set[i].posz == SourceZ::EXTNAV) {
// ExtNav could potentially be used, so align it
align_posz = true;
break;
}
}
}
visual_odom->align_position_to_ahrs(align_posxy, align_posz);
#endif
}
3.13 usingGPS
如果任何来源使用 GPS，则返回 true
// sensor specific helper functions
bool AP_NavEKF_Source::usingGPS() const
{
return getPosXYSource() == SourceXY::GPS ||
getPosZSource() == SourceZ::GPS ||
getVelXYSource() == SourceXY::GPS ||
getVelZSource() == SourceZ::GPS ||
getYawSource() == SourceYaw::GSF;
}
3.14 configured
检查参数是否已配置
// true if some parameters have been configured (used during parameter conversion)
bool AP_NavEKF_Source::configured()
{
if (_configured) {
return true;
}
// first source parameter is used to determine if configured or not
_configured = _source_set[0].posxy.configured();
return _configured;
}
3.15 mark_configured
标记参数为已配置
// mark parameters as configured (used to ensure parameter conversion is only done once)
void AP_NavEKF_Source::mark_configured()
{
// save first parameter's current value to mark as configured
return _source_set[0].posxy.save(true);
}
3.16 pre_arm_check
执行预备检查
// 如果我们在启用检查中失败，则返回 false，此时缓冲区将填充失败消息
// 如果需要检查垂直或水平位置配置，则 requires_position 应为 true
bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
{
auto &dal = AP::dal();
bool baro_required = false;
bool beacon_required = false;
bool compass_required = false;
bool gps_required = false;
bool rangefinder_required = false;
bool visualodom_required = false;
bool optflow_required = false;
bool wheelencoder_required = false;
// 检查源参数是否有效
for (uint8_t i = 0; i < AP_NAKEKF_SOURCE_SET_MAX; i++) {
if (requires_position) {
// 检查 posxy
switch ((SourceXY)_source_set[i].posxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::BEACON:
beacon_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::OPTFLOW:
case SourceXY::WHEEL_ENCODER:
default:
// 无效的 posxy 值
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i + 1);
return false;
}
// 检查 velxy
switch ((SourceXY)_source_set[i].velxy.get()) {
case SourceXY::NONE:
break;
case SourceXY::GPS:
gps_required = true;
break;
case SourceXY::OPTFLOW:
optflow_required = true;
break;
case SourceXY::EXTNAV:
visualodom_required = true;
break;
case SourceXY::WHEEL_ENCODER:
wheelencoder_required = true;
break;
case SourceXY::BEACON:
default:
// 无效的 velxy 值
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i + 1);
return false;
}
// 检查 posz
switch ((SourceZ)_source_set[i].posz.get()) {
case SourceZ::BARO:
baro_required = true;
break;
case SourceZ::RANGEFINDER:
rangefinder_required = true;
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::BEACON:
beacon_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::NONE:
break;
default:
// 无效的 posz 值
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i + 1);
return false;
}
// 检查 velz
switch ((SourceZ)_source_set[i].velz.get()) {
case SourceZ::NONE:
break;
case SourceZ::GPS:
gps_required = true;
break;
case SourceZ::EXTNAV:
visualodom_required = true;
break;
case SourceZ::BARO:
case SourceZ::RANGEFINDER:
case SourceZ::BEACON:
default:
// 无效的 velz 值
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i + 1);
return false;
}
}
// 检查 yaw
switch ((SourceYaw)_source_set[i].yaw.get()) {
case SourceYaw::NONE:
case SourceYaw::GPS:
// 有效的 yaw 值
break;
case SourceYaw::COMPASS:
// 跳过罗盘检查以便用户更容易设置无罗盘操作
break;
case SourceYaw::GPS_COMPASS_FALLBACK:
compass_required = true;
break;
case SourceYaw::EXTNAV:
visualodom_required = true;
break;
case SourceYaw::GSF:
gps_required = true;
break;
default:
// 无效的 yaw 值
hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i + 1);
return false;
}
}
// 检查所有必需的传感器是否可用
const char* ekf_requires_msg = "EK3 sources require %s";
if (baro_required && (dal.baro().num_instances() == 0)) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Baro");
return false;
}
if (beacon_required) {
#if AP_BEACON_ENABLED
const bool beacon_available = (dal.beacon() != nullptr && dal.beacon()->enabled());
#else
const bool beacon_available = false;
#endif
if (!beacon_available) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Beacon");
return false;
}
}
if (compass_required && (dal.compass().get_num_enabled() == 0)) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Compass");
return false;
}
if (gps_required && (dal.gps().num_sensors() == 0)) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "GPS");
return false;
}
if (optflow_required && !dal.opticalflow_enabled()) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "OpticalFlow");
return false;
}
if (rangefinder_required) {
#if AP_RANGEFINDER_ENABLED
const bool have_rangefinder = (dal.rangefinder() != nullptr && dal.rangefinder()->has_orientation(ROTATION_PITCH_270));
#else
const bool have_rangefinder = false;
#endif
if (!have_rangefinder) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "RangeFinder");
return false;
}
}
if (visualodom_required) {
bool visualodom_available = false;
#if HAL_VISUALODOM_ENABLED
auto *vo = AP::dal().visualodom();
visualodom_available = vo && vo->enabled();
#endif
if (!visualodom_available) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "VisualOdom");
return false;
}
}
if (wheelencoder_required && !dal.wheelencoder_enabled()) {
hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "WheelEncoder");
return false;
}
return true;
}
3.17 ext_nav_enabled
检查是否在任何来源中启用了外部导航
// return true if ext nav is enabled on any source
bool AP_NavEKF_Source::ext_nav_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
if (src.posxy == SourceXY::EXTNAV) {
return true;
}
if (src.posz == SourceZ::EXTNAV) {
return true;
}
if (src.velxy == SourceXY::EXTNAV) {
return true;
}
if (src.velz == SourceZ::EXTNAV) {
return true;
}
if (src.yaw == SourceYaw::EXTNAV) {
return true;
}
}
return false;
}
3.18 gps_yaw_enabled
检查是否在任何来源中启用了 GPS 航向
// return true if GPS yaw is enabled on any source
bool AP_NavEKF_Source::gps_yaw_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
const SourceYaw yaw = SourceYaw(src.yaw.get());
if (yaw == SourceYaw::GPS ||
yaw == SourceYaw::GPS_COMPASS_FALLBACK) {
return true;
}
}
return false;
}
3.19 wheel_encoder_enabled
检查是否在任何来源中启用了车轮编码器
// return true if wheel encoder is enabled on any source
bool AP_NavEKF_Source::wheel_encoder_enabled(void) const
{
for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
const auto &src = _source_set[i];
if (src.velxy == SourceXY::WHEEL_ENCODER) {
return true;
}
}
return false;
}
3.20 get_active_source_set
返回当前激活的来源集的索引
// returns active source set
uint8_t AP_NavEKF_Source::get_active_source_set() const
{
return active_source_set;
}
4. 总结
AP_NavEKF_Source 类是一个导航或控制系统的一部分，可能用于无人机（UAV）或其他机器人系统。
它管理不同的导航数据来源，包括位置、速度和航向信息。
该类与其他组件一起使用，以提供一个可靠的导航解决方案。
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
834
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
