开源飞控APM备降点源码分析

备降点

在航迹规划中，为了预防突发事件，一般会设定几个备降点，也可以把它理解成返航点。在开源飞控中我们原本航线中设定的降落点可以是home点，而备降点的作用是除home点外，无人机也可以触发降落的地方。这里，我也是因为涉及到备降点的需求，才需要对开源飞控做二次开发。

开源飞机备降点（集结点）

如下图所示，用QGC地面站，在设置航线时，可以选择集结点，这个集结点也就是前面所谓的备降点，一个R的小图标。在实际测试中会发现，只要有备降点，当触发返航RTL时，无人机会优先选择最近的备降点降落，而不是直接返回home点。这与我们实际项目中的需求不符合，我们更希望的是，这个备降点的触发不是自动的，而是可控的，也就是在没有人为干扰下，无人机默认是返回设定的home点降落。

源码

备降点（集结点）的源码在 AP_Rally 库中，主要分为以下几个部分：

1.集结点数据

cpp 复制代码

// 存储的集结点结构（
struct RallyLocation {
    int32_t lat;        // 纬度
    int32_t lng;        // 经度
    int16_t alt;        // 海拔（米）
    uint8_t alt_frame:4; // 海拔参考系
    uint8_t alt_frame_valid:1; // 海拔参考系是否有效
   
};

2.配置参数

cpp 复制代码

// 参数定义
AP_GROUPINFO("TOTAL", 0, AP_Rally, _rally_point_total_count, 0),       // 集结点总数
AP_GROUPINFO("LIMIT_KM", 1, AP_Rally, _rally_limit_km, RALLY_LIMIT_KM_DEFAULT), // 集结点限制距离
AP_GROUPINFO("INCL_HOME", 2, AP_Rally, _rally_incl_home, RALLY_INCLUDE_HOME_DEFAULT), // 是否包含Home点

3.核心算法

首先获取集结点的数据

cpp 复制代码

bool AP_Rally::get_rally_point_with_index(uint8_t i, RallyLocation &ret) const
{
    if (i >= (uint8_t) _rally_point_total_count) {
        return false;  // 索引超出范围
    }

    // 从存储读取数据
    _storage.read_block(&ret, i * sizeof(RallyLocation), sizeof(RallyLocation));

    if (ret.lat == 0 && ret.lng == 0) {
        return false;
    }

    return true;
}

查找最近的集结点

cpp 复制代码

bool AP_Rally::find_nearest_rally_point(const Location &current_loc, 
                                        RallyLocation &return_loc) const
{
    float min_dis = -1;  // 最小距离初始值

    // 遍历所有集结点
    for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) {
        RallyLocation next_rally;
        if (!get_rally_point_with_index(i, next_rally)) {
            continue;  
        }
        
        // 转换为Location对象
        Location rally_loc = rally_location_to_location(next_rally);
        
        // 计算到当前点的距离
        float dis = current_loc.get_distance(rally_loc);

        // 检查是否更近且有效
        if (is_valid(rally_loc) && (dis < min_dis || min_dis < 0)) {
            min_dis = dis;
            return_loc = next_rally;  // 更新最近集结点
        }
    }

    // 距离限制检查
    if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f)) {
        return false;  // 超出限制，使用Home点
    }

    // 返回是否找到有效集结点
    return min_dis >= 0;
}

计算最近返航点

cpp 复制代码

Location AP_Rally::calc_best_rally_or_home_location(
    const Location &current_loc, 
    float rtl_home_alt_amsl_cm) const
{
    // 默认返回Home点
    Location return_loc { AP::ahrs().get_home() };
    return_loc.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE);

    // 查找最近集结点
    RallyLocation ral_loc;
    if (find_nearest_rally_point(current_loc, ral_loc)) {
        Location loc = rally_location_to_location(ral_loc);
        
        // 判断是否使用集结点而不是Home点
        if (!_rally_incl_home  ||  // 配置不包含Home点
            (current_loc.get_distance(loc) < current_loc.get_distance(return_loc))) {
            return_loc = rally_location_to_location(ral_loc);
        }
    }

    return return_loc;
}

添加新集结点

cpp 复制代码

bool AP_Rally::append(const RallyLocation &loc)
{
    const uint8_t current_total = get_rally_total();
    
    // 增加总数
    _rally_point_total_count.set_and_save_ifchanged(current_total + 1);
    
    // 尝试存储集结点
    if (!set_rally_point_with_index(current_total, loc)) {
        // 失败时恢复总数
        _rally_point_total_count.set_and_save_ifchanged(current_total);
        return false;
    }
    
    return true;
}

总结

基本逻辑如下：

RTL触发返航→调用集结点功能函数→查找最近集结点→如果找到且符合条件就返回集结点，否则返回Home点。

而我这里是触发围栏，也就是会触发RTL返航来测试。

所以如果需要修改，也很简单，也就是改核心算法部分这里的判断逻辑，把home点的优先级提高，而不是就近返回集结点，后面有时间我再写一下我修改后的测试吧。