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

/*

  根据目的地的高度与指定起始高度之间的差异，重新设置用于滑降道的高度偏移量。

  如果目的地高于起始高度，则结果为正数。

 */

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

    /*

      如果该位置设置了terrain_alt标志，

      并且我们知道当前位置的地形高度，

      则将其视为地形高度处理。

     */

    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。这用于下滑道计算。

如果我们不进行地形跟随，那么"above"就很简单，它只是一个气压高度是否高于另一个的问题。

当处于地形跟随模式时，"above"表示地形上的当前高度高于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

}

/*

  返回根据ALT_OFFSET调整后的current_loc.alt。

  在长时间飞行中，通过考虑来自地面控制站的气压计变化

  或调整长时间任务的飞行高度，这将非常有用。

 */

int32_t Plane::adjusted_altitude_cm(void)

{

    return current_loc.alt - (mission_alt_offset()*100);

}

/*

  返还根据ALT_OFFSET调整得到的家位置相对高度。这在长距离飞行中很有用，可以考

  虑到GCS的气压变化、 或在长期飞行任务中调整飞行高度。

 */

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

        // we're not moving

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

   

        // 连续五秒没有测距仪的数据，别用它。

        return 0;

    }

g

    // 目前仅只支持使用测距仪进行着陆。

    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.2秒）时，

        // 我们认为我们完全处于范围之内。5％的变化是为了捕捉给出恒定范围的Lidar，

        // 无论是由于错误配置还是故障传感器。

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

                // 如果校正范围变化超过30m，重置Lidar的使用。我们可能有故障的Lidar。

                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));

            }

        }

       

    }

}

/*

  在目前的控制模式下，判断非自动地形禁用功能是否被激活并被允许。

 */

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