ardupilot/arduplane/altitude.cpp 高度保持程序/地形跟踪控制解析

本文链接：https://blog.csdn.net/qq_17031687/article/details/127879965

此篇博客详细介绍了飞行器如何根据飞行模式自动调整高度目标，同时考虑到地形跟踪和各种飞行阶段的需求，如滑翔、着陆和定位控制。它涵盖了高度保持策略，包括FBWB、Cruise模式下的处理，以及地形跟随、目标锁定和爬升/下降控制。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

#include "Plane.h"

/*
高度保持程序。这些系统可以同时应对气压控制和地形跟踪控制
*/

/*
根据飞行模式调整高度目标
*/
void Plane::adjust_altitude_target()
{
Location target_location;

if (control_mode == &mode_fbwb ||
control_mode == &mode_cruise) {
return;
}
if ((control_mode == &mode_loiter) && plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
return;
}
#if OFFBOARD_GUIDED == ENABLED
if (control_mode == &mode_guided && ((guided_state.target_alt_time_ms != 0) || guided_state.target_alt > -0.001 )) {

uint32_t now = AP_HAL::millis();
float delta = 1e-3f * (now - guided_state.target_alt_time_ms);
guided_state.target_alt_time_ms = now;

float delta_amt_f = delta * guided_state.target_alt_accel;
// 缩放100倍以匹配最后的目标高度，并转换为带符号的int32_
int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f);
Location temp {};
temp.alt = guided_state.last_target_alt + delta_amt_i; // …以避免此处出现浮动,
if (is_positive(guided_state.target_alt_accel)) {
temp.alt = MIN(guided_state.target_alt, temp.alt);
} else {
temp.alt = MAX(guided_state.target_alt, temp.alt);
}
guided_state.last_target_alt = temp.alt;
set_target_altitude_location(temp);
} else
#endif // OFFBOARD_GUIDED == ENABLED
if (control_mode->update_target_altitude()) {

} else if (landing.is_flaring()) {
//在紧急着陆时，使用TECS_LAND_SINK作为目标下沉速率，忽略目标高度
set_target_altitude_location(next_WP_loc);
} else if (landing.is_on_approach()) {
landing.setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude.offset_cm);
landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm);
} else if (landing.get_target_altitude_location(target_location)) {
set_target_altitude_location(target_location);
#if HAL_SOARING_ENABLED
} else if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
// 将目标高度重置为当前高度，以防止滑翔时出现较大的高度误差。
set_target_altitude_location(current_loc);
reset_offset_altitude();
#endif
} else if (reached_loiter_target()) {
// 一旦到达定点目标，然后锁定到最后的高度目标
set_target_altitude_location(next_WP_loc);
} else if (target_altitude.offset_cm != 0 &&
!current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
// 控制爬升/下降率
set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion);

// 保持在海拔的起点和终点高度范围内
constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
} else {
set_target_altitude_location(next_WP_loc);
}

altitude_error_cm = calc_altitude_error_cm();
}

/*
设置到下一个航路点的逐渐下滑道（如适用）
*/
void Plane::setup_glide_slope(void)
{
// 确定到下一个航路点的距离，以计算高度变化率
auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
TECS_controller.set_path_proportion(auto_state.wp_proportion);
update_flight_stage();

/*
确定是否会逐渐改变海拔高度，或者尽量尽快到达新的海拔高度。
*/
switch (control_mode->mode_number()) {
case Mode::Number::RTL:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
/* 如果高于目标高度，则缓慢下滑，但如果低于目标高度，上升速度会更快。参见https://github.com/ArduPilot/ardupilot/issues/39
*/
if (above_location_current(next_WP_loc)) {
set_offset_altitude_location(prev_WP_loc, next_WP_loc);
} else {
reset_offset_altitude();
}
break;

case Mode::Number::AUTO:
// 只有在20米以上或下降时，才会在自动模式下进行滑翔操作。20米的门槛是任意设定的，目的是为了防止在低空飞行时可能撞上障碍物。
if (adjusted_relative_altitude_cm() > 2000 || above_location_current(next_WP_loc)) {
set_offset_altitude_location(prev_WP_loc, next_WP_loc);
} else {
reset_offset_altitude();
}
break;
default:
reset_offset_altitude();
break;
}
}

/*
以当前高度作为RTL模式的飞行高度
*/
int32_t Plane::get_RTL_altitude_cm() const
{
if (g.RTL_altitude_cm < 0) {
return current_loc.alt;
}
return g.RTL_altitude_cm + home.alt;
}

/*
以米为单位返回相对高度（相对于地形，如果可用，或返回原点）
*/
float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
{
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
}

#if HAL_QUADPLANE_ENABLED
if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) {
// 垂起构型着陆时测量值低于最小值时的一种特殊情况。假设我们离地面的高度为零
return 0;
}
#endif

#if AP_TERRAIN_AVAILABLE
float altitude;
if (target_altitude.terrain_following &&
terrain.status() == AP_Terrain::TerrainStatusOK &&
terrain.height_above_terrain(altitude, true)) {
return altitude;
}
#endif

#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_descent() &&
!quadplane.landing_with_fixed_wing_spiral_approach()) {
//当进行垂直起降着陆时，可以使用航路点高度作为地面高度而不能用LAND_FW_APPROACH高度。因为LAND_FW_APPROACH使用wp高度作为进场高度
return height_above_target();
}
#endif

return relative_altitude;
}

/*
将目标高度设置为当前高度。这在设置高度保持时使用，例如在CRUISE模式下释放油门时。
*/
void Plane::set_target_altitude_current(void)
{
// 将当前时间的海拔高度记录为目标高度
target_altitude.amsl_cm = current_loc.alt;

//重置任何情况下的下滑道偏移
reset_offset_altitude();

#if AP_TERRAIN_AVAILABLE
// 如果可能，则记录地形高度
float terrain_altitude;
if (terrain_enabled_in_current_mode() && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) {
target_altitude.terrain_following = true;
target_altitude.terrain_alt_cm = terrain_altitude*100;
} else {
// 果地形跟随被禁用，或者我们在设置海拔高度时不知道我们的地形高度，那么不要跟随地形
target_altitude.terrain_following = false;
}
#endif
}

/*
将目标高度设置为当前高度,并使用ALT_OFFSET进行适应调节。
*/
void Plane::set_target_altitude_current_adjusted(void)
{
set_target_altitude_current();

// 使用adjusted_altitude_cm()时将 ALTITUDE_OFFSET也考虑在内
target_altitude.amsl_cm = adjusted_altitude_cm();
}

/*
基于位置结构设置目标高度
*/
void Plane::set_target_altitude_location(const Location &loc)
{
target_altitude.amsl_cm = loc.alt;
if (loc.relative_alt) {
target_altitude.amsl_cm += home.alt;
}
#if AP_TERRAIN_AVAILABLE
/*
如果该位置设置了地形高度标志，并且知道当前位置的地形高度，则将其视为地形高度
*/
float height;
if (loc.terrain_alt && terrain.height_above_terrain(height, true)) {
target_altitude.terrain_following = true;
target_altitude.terrain_alt_cm = loc.alt;
if (!loc.relative_alt) {

target_altitude.terrain_alt_cm -= home.alt;
}
} else {
target_altitude.terrain_following = false;
}
#endif
}

/*
返回相对于主目标高度的厘米数。用于高度控制库
*/
int32_t Plane::relative_target_altitude_cm(void)
{
#if AP_TERRAIN_AVAILABLE
float relative_home_height;
if (target_altitude.terrain_following &&
terrain.height_relative_home_equivalent(target_altitude.terrain_alt_cm*0.01f,
relative_home_height, true)) {
// 添加前置自适应调整目标高度
target_altitude.lookahead = lookahead_adjustment();
relative_home_height += target_altitude.lookahead;

// 传感器测量数据正确
relative_home_height += rangefinder_correction();

// 在跟踪地形时，如果有当前位置的地形数据，则使用地形跟踪功能。
return relative_home_height*100;
}
#endif
int32_t relative_alt = target_altitude.amsl_cm - home.alt;
relative_alt += mission_alt_offset()*100;
relative_alt += rangefinder_correction() * 100;
return relative_alt;
}

/*
以厘米为单位更改当前目标高度。用于应对因CRUISE或FBWB中的油门引起的变化
*/
void Plane::change_target_altitude(int32_t change_cm)
{
target_altitude.amsl_cm += change_cm;
#if AP_TERRAIN_AVAILABLE
if (target_altitude.terrain_following && !terrain_disabled()) {
target_altitude.terrain_alt_cm += change_cm;
}
#endif
}
/*
按目标高度偏移量的比例改变目标高度（与前一个WP之间的高度差）。比例应介于0和1之间。

当比例为零时，已经到达目的地。当比例为1时，处于起始航路点。

请注意，target_altitude最初是根据目的地航路点设置的
*/
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
{
set_target_altitude_location(loc);
proportion = constrain_float(proportion, 0.0f, 1.0f);
change_target_altitude(-target_altitude.offset_cm*proportion);
//如果在航线上方并且应该爬坡，就重建飞行坡度
if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc);
set_offset_altitude_location(current_loc, loc);
change_target_altitude(-target_altitude.offset_cm*proportion);
// 调整新的目标前置高度，以适应已经飞过的部分
if(proportion > 0.0f)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
}
}
}

/*
将目标高度限制在两个位置之间。用于确保在两个航路点内高度的保持
*/
void Plane::constrain_target_altitude_location(const Location &loc1, const Location &loc2)
{
if (loc1.alt > loc2.alt) {
target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt);
} else {
target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc1.alt, loc2.alt);
}
}

/*
计算目标高度和当前高度之间的误差
*/
int32_t Plane::calc_altitude_error_cm(void)
{
#if AP_TERRAIN_AVAILABLE
float terrain_height;
if (target_altitude.terrain_following &&
terrain.height_above_terrain(terrain_height, true)) {
return target_altitude.lookahead*100 + target_altitude.terrain_alt_cm - (terrain_height*100);
}
#endif
return target_altitude.amsl_cm - adjusted_altitude_cm();
}

/*
检查是否低于 FBWB_min_altitude_cm
*/
void Plane::check_fbwb_minimum_altitude(void)
{
if (g.FBWB_min_altitude_cm == 0) {
return;
}

#if AP_TERRAIN_AVAILABLE
if (target_altitude.terrain_following) {
// 将目标地形高度设置为最小值
if (target_altitude.terrain_alt_cm < g.FBWB_min_altitude_cm) {
target_altitude.terrain_alt_cm = g.FBWB_min_altitude_cm;
}
return;
}
#endif

if (target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm;
}
}

/*
重置高度偏差以用于建立坡度
*/
void Plane::reset_offset_altitude(void)
{
target_altitude.offset_cm = 0;
}

/*
根据目的地的高度和指定的起始高度之间的差异，重置高度偏差以用于建立坡度。如果目的地高于起始高度，则结果为正。
*/
void Plane::set_offset_altitude_location(const Location &start_loc, const Location &destination_loc)
{
target_altitude.offset_cm = destination_loc.alt - start_loc.alt;

#if AP_TERRAIN_AVAILABLE
/*
如果该位置设置了地形标志，并且知道当前位置的地形高度，则将其视为地形高度
*/
float height;
if (destination_loc.terrain_alt &&
target_altitude.terrain_following &&
terrain.height_above_terrain(height, true)) {
target_altitude.offset_cm = target_altitude.terrain_alt_cm - (height * 100);
}
#endif

if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
//如果在目标高度的GLIDE_SLOPE_MIN米范围内，则将误差修正为不使用高度纠偏。
//这允许更精确的飞行任务，其中飞机可能由于局部地形变化而在航路点转弯点附近失去或增加一点高度
if (g.glide_slope_min <= 0 ||
labs(target_altitude.offset_cm)*0.01f < g.glide_slope_min) {
target_altitude.offset_cm = 0;
}
}
}

/*
如果current_loc高于loc，则返回true。用于坡度计算。
如果不跟随地形，“高于”则简单的意味着一个的气压高度高于另一个。
当处于地形跟随模式时，“高于”表示当前的地形高度高于loc的地形高度。
如果current_loc位于地势较低的地方，那么当处于较低的气压高度时，current_ loc很可能“高于”loc
*/
bool Plane::above_location_current(const Location &loc)
{
#if AP_TERRAIN_AVAILABLE
float terrain_alt;
if (loc.terrain_alt &&
terrain.height_above_terrain(terrain_alt, true)) {
float loc_alt = loc.alt*0.01f;
if (!loc.relative_alt) {
loc_alt -= home.alt*0.01f;
}
return terrain_alt > loc_alt;
}
#endif

float loc_alt_cm = loc.alt;
if (loc.relative_alt) {
loc_alt_cm += home.alt;
}
return current_loc.alt > loc_alt_cm;
}

/*
如果启用terrain_FOLLOW，则修改要设置的地形跟随目的地
*/
void Plane::setup_terrain_target_alt(Location &loc) const
{
#if AP_TERRAIN_AVAILABLE
if (terrain_enabled_in_current_mode()) {
loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN);
}
#endif
}

/*
返回 current_loc.alt 以适应 ALT_OFFSET
这在长时间飞行中很有用，可以适应地面环境变化引起的气压变化，也可以适应长时间飞行任务的飞行高度
*/
int32_t Plane::adjusted_altitude_cm(void)
{
return current_loc.alt - (mission_alt_offset()*100);
}

/*
为ALT_OFFSET调整的返航相对高度。
在长距离飞行中，可以适应地面环境变化引起的气压变化，也可以适应长时间飞行任务的飞行高度
*/
int32_t Plane::adjusted_relative_altitude_cm(void)
{
return (relative_altitude - mission_alt_offset())*100;
}

/*
返回任务高度偏移。这会提高或降低所有任务项目。
它主要使用ALT_OFFSET参数设置，
但如果我们中止了硬着陆，也可以通过NAV_LAND命令的测距仪着陆代码进行调整
*/
float Plane::mission_alt_offset(void)
{
float ret = g.alt_offset;
if (control_mode == &mode_auto &&
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) {
//当由于偏离下滑线太高而中止着陆后着陆时，使用上次着陆尝试的偏移量
ret += landing.alt_offset;
}
return ret;
}

/*
返回下一个WP_loc高度以上的高度（米）
*/
float Plane::height_above_target(void)
{
float target_alt = next_WP_loc.alt*0.01;
if (!next_WP_loc.relative_alt) {
target_alt -= ahrs.get_home().alt*0.01f;
}

#if AP_TERRAIN_AVAILABLE
// 如果可能记录地形高度
float terrain_altitude;
if (next_WP_loc.terrain_alt &&
terrain.height_above_terrain(terrain_altitude, true)) {
return terrain_altitude - target_alt;
}
#endif

return (adjusted_altitude_cm()*0.01f - ahrs.get_home().alt*0.01f) - target_alt;
}

/*
根据地形预测调整计算目标高度
*/
float Plane::lookahead_adjustment(void)
{
#if AP_TERRAIN_AVAILABLE
int32_t bearing_cd;
int16_t distance;
// 计算与目标的距离和方位
if (control_mode == &mode_fbwb) {
// FBWB中没有目标航路点，因此使用航向角作为近似值
bearing_cd = ahrs.yaw_sensor;
distance = g.terrain_lookahead;
} else if (!reached_loiter_target()) {
bearing_cd = nav_controller->target_bearing_cd();
distance = constrain_float(auto_state.wp_distance, 0, g.terrain_lookahead);
} else {
// 定高时无前置反馈
bearing_cd = 0;
distance = 0;
}
if (distance <= 0) {
// 无前置反馈
return 0;
}

float groundspeed = ahrs.groundspeed();
if (groundspeed < 1) {
// 没有移动
return 0;
}

// 们需要知道爬升率,们使用50%的最大爬升率。不会一直保持100%的油门，并在爬升时上提供更多的余量。
float climb_ratio = 0.5f * TECS_controller.get_max_climbrate() / groundspeed;

if (climb_ratio <= 0) {
// 对于负爬升率，前馈补偿是没有意义的
return 0;
}

// 查询地形库以进行高度前馈
float lookahead = terrain.lookahead(bearing_cd*0.01f, distance, climb_ratio);

if (target_altitude.offset_cm < 0) {
// 正朝着航路点前进，所以不需要爬那么多
lookahead += target_altitude.offset_cm*0.01f;
}

// 将前馈补偿限制在合理的限度内
return constrain_float(lookahead, 0, 1000.0f);
#else
return 0;
#endif
}

/*
使用传感器数据校正目标高度。
返回以米为单位的偏差以校正目标高度。
正数表示需要要求速度/高度控制器使得无人机飞得更高
*/
float Plane::rangefinder_correction(void)
{
if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
// 5s没有接收到测距仪数据，放弃使用该传感器数据
return 0;
}

// 目前仅支持气压测距着陆
bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (!using_rangefinder) {
return 0;
}

return rangefinder_state.correction;
}

/*
NAV_LAND点和当前位置之间的地形高度差的测距仪数据
*/
void Plane::rangefinder_terrain_correction(float &height)
{
#if AP_TERRAIN_AVAILABLE
if (!g.rangefinder_landing ||
flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND ||
!terrain_enabled_in_current_mode()) {
return;
}
float terrain_amsl1, terrain_amsl2;
if (!terrain.height_amsl(current_loc, terrain_amsl1) ||
!terrain.height_amsl(next_WP_loc, terrain_amsl2)) {
return;
}
float correction = (terrain_amsl1 - terrain_amsl2);
height += correction;
auto_state.terrain_correction = correction;
#endif
}

/*
更新测量高度和地形高度之间的误差
*/
void Plane::rangefinder_height_update(void)
{
float distance = rangefinder.distance_orient(ROTATION_PITCH_270);

if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && ahrs.home_is_set()) {
if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true;
rangefinder_state.initial_range = distance;
}
// 校正姿态范围(乘以 DCM.c.z, 即 cos(roll)*cos(pitch)
rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;

rangefinder_terrain_correction(rangefinder_state.height_estimate);

// 当我们有10个初始范围相差最大范围的5%的样本（0.2s）时，认为自己完全在范围内。
// 5%的变化范围是由于激光雷达的系统误差或环境因素而引起的测量误差
if (rangefinder_state.in_range_count < 10) {
if (!is_equal(distance, rangefinder_state.last_distance) &&
fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01f) {
rangefinder_state.in_range_count++;
}
if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01*0.2) {
// 超过满量程20%,重置计数器
rangefinder_state.in_range_count = 0;
}
} else {
rangefinder_state.in_range = true;
bool flightstage_good_for_rangefinder_landing = false;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
flightstage_good_for_rangefinder_landing = true;
}
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
(control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) {
flightstage_good_for_rangefinder_landing = true;
}
#endif
if (!rangefinder_state.in_use &&
flightstage_good_for_rangefinder_landing &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
}
}
rangefinder_state.last_distance = distance;
} else {
rangefinder_state.in_range_count = 0;
rangefinder_state.in_range = false;
}

if (rangefinder_state.in_range) {

// 基准校正是气压高度和测距仪估计值之间的差值
float correction = adjusted_relative_altitude_cm()*0.01 - rangefinder_state.height_estimate;

#if AP_TERRAIN_AVAILABLE
// 如果启用了地形跟踪，那么校正是基于地形数据
float terrain_altitude;
if ((target_altitude.terrain_following || terrain_enabled_in_current_mode()) &&
terrain.height_above_terrain(terrain_altitude, true)) {
correction = terrain_altitude - rangefinder_state.height_estimate;
}
#endif

// 记住最后一次更正。当旧数据少于5秒时使用低通滤波器
uint32_t now = millis();
if (now - rangefinder_state.last_correction_time_ms > 5000) {
rangefinder_state.correction = correction;
rangefinder_state.initial_correction = correction;
if (g.rangefinder_landing) {
landing.set_initial_slope();
}
rangefinder_state.last_correction_time_ms = now;
} else {
rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
rangefinder_state.last_correction_time_ms = now;
if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {
// 校正改变了超过30米，重新使用激光雷达，防止激光雷达的故障情况
if (rangefinder_state.in_use) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder disengaged at %.2fm", (double)rangefinder_state.height_estimate);
}
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
}
}

}
}

/*
determine if Non Auto Terrain Disable is active and allowed in present control mode
*/
bool Plane::terrain_disabled()
{
return control_mode->allows_terrain_disable() && non_auto_terrain_disable;
}

/*
检查当前模式是否启用了地形跟随
*/
#if AP_TERRAIN_AVAILABLE
const Plane::TerrainLookupTable Plane::Terrain_lookup[] = {
{Mode::Number::FLY_BY_WIRE_B, terrain_bitmask::FLY_BY_WIRE_B},
{Mode::Number::CRUISE, terrain_bitmask::CRUISE},
{Mode::Number::AUTO, terrain_bitmask::AUTO},
{Mode::Number::RTL, terrain_bitmask::RTL},
{Mode::Number::AVOID_ADSB, terrain_bitmask::AVOID_ADSB},
{Mode::Number::GUIDED, terrain_bitmask::GUIDED},
{Mode::Number::LOITER, terrain_bitmask::LOITER},
{Mode::Number::CIRCLE, terrain_bitmask::CIRCLE},
#if HAL_QUADPLANE_ENABLED
{Mode::Number::QRTL, terrain_bitmask::QRTL},
{Mode::Number::QLAND, terrain_bitmask::QLAND},
{Mode::Number::QLOITER, terrain_bitmask::QLOITER},
#endif
};

bool Plane::terrain_enabled_in_current_mode() const
{
return terrain_enabled_in_mode(control_mode->mode_number());
}

bool Plane::terrain_enabled_in_mode(Mode::Number num) const
{
// 全局启用
if ((g.terrain_follow.get() & int32_t(terrain_bitmask::ALL)) != 0) {
return true;
}

// 特定启用
for (const struct TerrainLookupTable entry : Terrain_lookup) {
if (entry.mode_num == num) {
if ((g.terrain_follow.get() & int32_t(entry.bitmask)) != 0) {
return true;
}
break;
}
}

return false;
}
#endif