ardupilot/Ardupilot V4.3.4 源码解析：altitude.cpp(1)

/*

   本程序是免费的：你可以根据免费软件基金会发布的GNU通用公共许可证的条

   款，即许可证的第3版，或（你选择）任何更高的版本，重新发布和/或修改它。

   本程序的发布是希望它能起到作用，但没有任何保证；甚至没有明确的质保

   或第三方保证。 更多细节请参见GNU通用公共许可证。你应该已经收到了一份GNU通用

   公共许可证的副本。的副本。 如果没有，请参阅<http://www.gnu.org/licenses/>。

 */

#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 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.

        // 离线高度要求

        uint32_t now = AP_HAL::millis();

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

        guided_state.target_alt_time_ms = now;

        // 以浮点数形式准确计算出 Δ（变化量）,然后按照 last_target_alt 进行 x100 的比例缩放，并将其转换为有符号的 int32_t 类型，因为它可能是负数。

        float delta_amt_f = delta * guided_state.target_alt_accel;

        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()) {

        // altitude 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米，或者在下降时。

        这是为了确保飞行的安全性，避免在低高度下缓慢升高引发潜在的危险。

        请注意，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高度，以平均海平面高度（AMSL）表示,单位为厘米。

 */

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;

}

/*

  返回相对高度（以米为单位）（如果有可用的，就根据于地形，否则就默认原点）。

  如果有可用的地形数据，您可以获取飞行器相对于地面的高度。否则，您可以获取相对于起飞点的高度。

  请注意，获取相对高度可能需要通过飞行控制系统或传感器来获取。这通常涉及地形雷达、高度计或GPS等设备。

 */

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()) {

        /*在进行垂直起降（VTOL）着陆时，可以将航点高度作为地面高度。

        但是，在使用LAND_FW_APPROACH（固定翼着陆逼近）模式时，不能将航点高度作为逼近高度。

        通常情况下，在VTOL着陆时，飞行器会使用航点高度作为地面高度的估计。

        这是因为航点高度通常代表了预设的着陆区域高度。

        然而，在使用LAND_FW_APPROACH模式时，它会利用航点高度作为逼近高度。

        逼近高度是指飞行器接近着陆区域开始进行固定翼飞行的高度。

        在这种情况下，航点高度被用作指导飞行器从垂直起降模式转换为固定翼模式的参考。

        因此，在LAND_FW_APPROACH模式下，不能将航点高度视为地面高度，而应将其视为逼近高度。*/

        return height_above_target();

    }

#endif

    return relative_altitude;

}

/*

设置目标高度为当前高度。这通常在设置高度保持（altitude hold）时使用，比如在CRUISE模式下释放升降舵。

在飞行中，高度保持是一种模式，允许飞行器维持特定的高度。

当进入高度保持模式时，您可以将当前的高度设定为目标高度，以便飞行器维持在相同的高度上飞行。

在CRUISE模式中，当您想要保持飞行器在特定高度上稳定飞行时，释放升降舵是常见的操作。

当释放升降舵后，飞行器将根据当前高度进行微调，以保持在当前高度上飞行。

通过将目标高度设置为当前高度，您可以告诉飞行控制系统或自动驾驶仪，在高度保持模式下，以当前高度为目标维持飞行。

这样可以帮助您在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 {

        // if terrain following is disabled, or we don't know our

        // terrain altitude when we set the altitude then don't

        // terrain follow

        // 如果禁用地形跟随，或者在设置高度时不知道我们的地形高度，则不进行地形跟随。

        target_altitude.terrain_following = false;        

    }

#endif

}

/*

    将目标高度设置为当前高度，并进行海拔偏移（ALT_OFFSET）调整。

    海拔偏移（ALT_OFFSET）是一种调整目标高度的技术，常用于校正飞行器的高度测量误差或考虑其他因素对目标高度的修正。

    下面是将目标高度设置为当前高度，并进行海拔偏移调整的步骤：

   1) 获取当前飞行器的海拔高度，确保使用准确的传感器或GPS系统来获取高度数据。

   2) 使用任何适用的修正因素，比如误差校正或其他相关情况，确定您希望应用的海拔偏移值。

   3) 将当前高度与海拔偏移值相加或相减，以得到调整后的目标高度。

   例如，如果当前高度为100米，您希望将目标高度提高10米，通过应用海拔偏移值，可以将目标高度设置为110米。

   或者，如果您希望将目标高度降低10米，您可以将目标高度设置为90米。

    请注意，具体的海拔偏移值和调整方式可能因不同的应用和需求而有所不同。在实际操作中，请根据情况和需求进行相应的调整。

 */

void Plane::set_target_altitude_current_adjusted(void)

{

    set_target_altitude_current();

    // use adjusted_altitude_cm() to take account of ALTITUDE_OFFSET

    // 使用调整后的海拔(单位是cm)，考虑海拔偏移。

    /*使用adjusted_altitude_cm()函数来考虑海拔偏移（ALTITUDE_OFFSET）。

    adjusted_altitude_cm()函数是一个用于计算修正后目标高度的函数，它可以根据给定的海拔偏移值来调整高度。

具体使用adjusted_altitude_cm()函数的步骤如下：

1) 获取当前高度并将其转换为厘米单位。确保使用准确的传感器或GPS系统来获取高度数据。

2) 根据应用的海拔偏移（ALTITUDE_OFFSET）值，确定您希望进行的高度调整。

3) 使用adjusted_altitude_cm()函数，将当前高度和海拔偏移值作为参数传入，得到修正后的目标高度。

例如，假设当前高度为1000米，您希望将目标高度提高100米，您可以使用adjusted_altitude_cm()函数如下：

adjusted_altitude_cm(1000, 100) // 将1000米的当前高度增加100米的海拔偏移

函数将返回修正后的目标高度，以厘米为单位。在这个例子中，结果为110000厘米。*/

   

    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

    /*

      如果位置结构中设置了terrain_alt标志，并且知道当前位置的地形海拔高度，则将其视为地形海拔高度。

要根据这个信息设置目标高度，可以按照以下步骤进行：

1) 检查位置结构中是否设置了terrain_alt标志。该标志表明给定的高度值应被视为地形海拔高度。

2) 如果terrain_alt标志已设置，则获取与当前位置对应的地形海拔高度。可以从地形数据库、海拔地图或其他可靠来源获取这个值。

3) 使用在上一步中获得的地形海拔高度来设置目标高度。

例如，如果位置结构提供了500米的高度，并且terrain_alt标志已设置，如果我们知道当前的地形海拔高度是200米，则应将目标高度设置为200米而不是500米。

请确保所使用的地形海拔数据准确且及时更新。不同的数据源和地形海拔计算方法可能会引入变化和不确定性。

     */

    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

}

/*

    以厘米为单位返回相对于家位置目标的高度。用于高度控制库。

    返回相对于家庭位置的目标高度，单位为厘米。用于高度控制库。

要计算相对于家庭位置的目标高度，需要按照以下步骤进行操作：

1) 获取目标高度和家庭位置的海拔高度。这些数据可以通过传感器、地图接口或其他方法获得。

2) 使用目标高度减去家庭位置的海拔高度，得到相对于家庭位置的高度差。

3) 将高度差转换为厘米。具体的转换方法可以根据所使用的高度控制库和单位系统进行调整。

以下是一个示例计算相对于家庭位置的目标高度的伪代码：

// 示例伪代码

targetAltitudeCM = (targetAltitude - homeAltitude) * 100;

这个示例代码假设目标高度和家庭位置的海拔高度是以米为单位的。通过将高度差乘以100，将其转换为厘米。

请根据您使用的具体编程语言和库进行适当的调整。

 */

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到下一个WP的高度差）。比率应该在0和1之间。

  当比率为0时，我们就到达了目的地。当比率为1时，我们则处于起始航点。

  请注意，目标高度最初是根据目的地航点设置的。

 */

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;

}