Ardupilot — AP_OpticalFlow代码梳理

news2024/11/24 8:00:54

文章目录

前言

1 Copter.cpp

1.1 void Copter::setup()

2 system.cpp

2.1 void Copter::init_ardupilot()

3 sensors.cpp

3.1 void Copter::init_optflow()

3.2 对象optflow说明

4 OpticalFlow.cpp

4.1 void OpticalFlow::init(uint32_t log_bit)

5 AP_OpticalFlow_Pixart.cpp

5.1 AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(...)

5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

5.3 void AP_OpticalFlow_Pixart::timer(void)

5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

5.5 SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),

5.6 void AP_OpticalFlow_Pixart::update(void)

前言

故事的开始，要从参数 FLOW_TYPE 说起。

FLOW_TYPE：光学流量传感器类型

RebootRequired

Values

True

Value

Meaning

0

None

1

PX4Flow

2

Pixart

3

Bebop

4

CXOF

5

MAVLink

6

UAVCAN

1 Copter.cpp

1.1 void Copter::setup()

此函数仅在启动时调用一次。用于初始化一些必要的任务。此函数由 HAL 中的 main() 函数调用。

void Copter::setup()
{
    // Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();

    // setup storage layout for copter
    StorageManager::set_layout_copter();

    init_ardupilot();

    // initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

2 system.cpp

2.1 void Copter::init_ardupilot()

init_ardupilot() 函数将处理空中重启所需的一切。稍后将确定飞行器是否真的在地面上，并在这种情况下处理地面启动。

void Copter::init_ardupilot()
{
    ...

    // init the optical flow sensor
    init_optflow();

    ...
}

3 sensors.cpp

3.1 void Copter::init_optflow()

初始化光流传感器。

// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
    // initialise optical flow sensor
    optflow.init(MASK_LOG_OPTFLOW);
#endif      // OPTFLOW == ENABLED
}

3.2 对象optflow说明

在 Copter.h 文件中，我们用 OpticalFlow 类定义了 optflow 对象。

    // Optical flow sensor
#if OPTFLOW == ENABLED
    OpticalFlow optflow;
#endif

4 OpticalFlow.cpp

4.1 void OpticalFlow::init(uint32_t log_bit)

所以，我们在跳转 init() 这个成员函数的时候，跳转到 OpticalFlow 类的 init() 函数。

根据参数 FLOW_TYPE，选择不同的光流传感器进行检测。

本例以 Pixart（2）作为示例。

void OpticalFlow::init(uint32_t log_bit)
{
     _log_bit = log_bit;

    // return immediately if not enabled or backend already created
    if ((_type == (int8_t)OpticalFlowType::NONE) || (backend != nullptr)) {
        return;
    }

    switch ((OpticalFlowType)_type.get()) {
    case OpticalFlowType::NONE:
        break;
    case OpticalFlowType::PX4FLOW:
        backend = AP_OpticalFlow_PX4Flow::detect(*this);
        break;
    case OpticalFlowType::PIXART:
        backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
        if (backend == nullptr) {
            backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
        }
        break;
    case OpticalFlowType::BEBOP:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
        backend = new AP_OpticalFlow_Onboard(*this);
#endif
        break;
    case OpticalFlowType::CXOF:
        backend = AP_OpticalFlow_CXOF::detect(*this);
        break;
    case OpticalFlowType::MAVLINK:
        backend = AP_OpticalFlow_MAV::detect(*this);
        break;
    case OpticalFlowType::UAVCAN:
#if HAL_WITH_UAVCAN
        backend = new AP_OpticalFlow_HereFlow(*this);
#endif
        break;
    case OpticalFlowType::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        backend = new AP_OpticalFlow_SITL(*this);
#endif
        break;
    }

    if (backend != nullptr) {
        backend->init();
    }
}

5 AP_OpticalFlow_Pixart.cpp

5.1 AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(...)

首先会根据传入的参数，new 一个 AP_OpticalFlow_Pixart 类对象。

然后，检测光流传感器的产品 ID，再配置传感器（sensor->setup_sensor()）。

// detect the device
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
{
    AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
    if (!sensor) {
        return nullptr;
    }
    if (!sensor->setup_sensor()) {
        delete sensor;
        return nullptr;
    }
    return sensor;
}

5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

先读取光流传感器的产品 ID，识别光流传感器类型。再根据类型的不同，写入不同的参数配置光流传感器。

最后，注册光流传感器的周期运行函数 timer()。

// setup the device
bool AP_OpticalFlow_Pixart::setup_sensor(void)
{
    ...
    // check product ID
    uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
    uint8_t id2;
    if (id1 == 0x3f) {
        id2 = reg_read(PIXART_REG_INV_PROD_ID);
    } else {
        id2 = reg_read(PIXART_REG_INV_PROD_ID2);
    }
    debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
    if (id1 == 0x3F && id2 == uint8_t(~id1)) {
        model = PIXART_3900;
    } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
        model = PIXART_3901;
    } else {
        debug("Not a recognised device\n");
        return false;
    }

    if (model == PIXART_3900) {
        srom_download();

        id = reg_read(PIXART_REG_SROM_ID);
        if (id != srom_id) {
            debug("Pixart: bad SROM ID: 0x%02x\n", id);
            return false;
        }

        reg_write(PIXART_REG_SROM_EN, 0x15);
        hal.scheduler->delay(10);

        crc = reg_read16u(PIXART_REG_DOUT_L);
        if (crc != 0xBEEF) {
            debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
            return false;
        }
    }

    if (model == PIXART_3900) {
        load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
    } else {
        load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
        hal.scheduler->delay(100);
        load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
    }

    hal.scheduler->delay(50);

    debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");

    integral.last_frame_us = AP_HAL::micros();

    _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
    return true;
}

5.3 void AP_OpticalFlow_Pixart::timer(void)

每 2ms 调用一次 timer() 函数，读取光流传感器的测量值。

可以将 #if 0 #endif 屏蔽，打开调试信息。

void AP_OpticalFlow_Pixart::timer(void)
{
    if (AP_HAL::micros() - last_burst_us < 500) {
        return;
    }
    motion_burst();
    last_burst_us = AP_HAL::micros();

    uint32_t dt_us = last_burst_us - integral.last_frame_us;
    float dt = dt_us * 1.0e-6;
    const Vector3f &gyro = AP::ahrs_navekf().get_gyro();

    {
        WITH_SEMAPHORE(_sem);

        integral.sum.x += burst.delta_x;
        integral.sum.y += burst.delta_y;
        integral.sum_us += dt_us;
        integral.last_frame_us = last_burst_us;
        integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
    }

#if 0
    static uint32_t last_print_ms;
    static int fd = -1;
    if (fd == -1) {
        fd = open("/dev/ttyACM0", O_WRONLY);
    }
    // used for debugging
    static int32_t sum_x;
    static int32_t sum_y;
    sum_x += burst.delta_x;
    sum_y += burst.delta_y;

    uint32_t now = AP_HAL::millis();
    if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
        last_print_ms = now;
        dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
               (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
               (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
        sum_x = sum_y = 0;
    }
#endif
}

5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

通过 SPI 读取光流传感器运动值。

void AP_OpticalFlow_Pixart::motion_burst(void)
{
    uint8_t *b = (uint8_t *)&burst;

    burst.delta_x = 0;
    burst.delta_y = 0;

    _dev->set_chip_select(true);
    uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;

    _dev->transfer(&reg, 1, nullptr, 0);
    hal.scheduler->delay_microseconds(150);

    for (uint8_t i=0; i<sizeof(burst); i++) {
        _dev->transfer(nullptr, 0, &b[i], 1);
        if (i == 0 && (burst.motion & 0x80) == 0) {
            // no motion, save some bus bandwidth
            _dev->set_chip_select(false);
            return;
        }
    }
    _dev->set_chip_select(false);
}

5.5 SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),

在 Copter.cpp 文件的周期任务列表中，注册了调用频率为 200Hz 的光流传感器 update() 函数。

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
...

#if OPTFLOW == ENABLED
    SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),
#endif
...
}

5.6 void AP_OpticalFlow_Pixart::update(void)

根据 5.1 中的返回的 AP_OpticalFlow_Pixart 对象，调用 AP_OpticalFlow_Pixart 类中的 update() 函数。

更新 - 从传感器读取最新数值，并填入 x、y 和总数。

// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Pixart::update(void)
{
    uint32_t now = AP_HAL::millis();
    if (now - last_update_ms < 100) {
        return;
    }
    last_update_ms = now;

    struct OpticalFlow::OpticalFlow_state state;
    state.surface_quality = burst.squal;

    if (integral.sum_us > 0) {
        WITH_SEMAPHORE(_sem);

        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
        float dt = integral.sum_us * 1.0e-6;

        state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
                                  integral.sum.y * flowScaleFactorY);
        state.flowRate *= flow_pixel_scaling / dt;

        // we only apply yaw to flowRate as body rate comes from AHRS
        _applyYaw(state.flowRate);

        state.bodyRate = integral.gyro / dt;

        integral.sum.zero();
        integral.sum_us = 0;
        integral.gyro.zero();
    } else {
        state.flowRate.zero();
        state.bodyRate.zero();
    }

    // copy results to front end
    _update_frontend(state);
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/995242.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

十三、函数式编程（1）

十三、函数式编程（1）

本章概要 新旧对比Lambda 表达式 递归 函数式编程语言操纵代码片段就像操作数据一样容易。 虽然 Java 不是函数式语言&#xff0c;但 Java 8 Lambda 表达式和方法引用 (Method References) 允许你以函数式编程。 在计算机时代早期&#xff0c;内存是稀缺和昂贵的。几乎每个人…
阅读更多...
手写Spring：第20章-事务处理

手写Spring：第20章-事务处理

文章目录 一、目标&#xff1a;事务处理二、设计&#xff1a;事务处理2.1 事务单元测试2.2 事务设计 三、实现&#xff1a;事务处理3.1 工程结构3.2 事务管理的核心类图3.3 定义事务注解3.3.1 定义事务注解3.3.2 定义事务接口3.3.3 默认事务定义实现类3.3.4 委托事务定义实现类…
阅读更多...
Java 多线程系列Ⅵ（并发编程的六大组件）

Java 多线程系列Ⅵ（并发编程的六大组件）

JUC 组件 前言一、Callable二、ReentrantLock三、Atomic 原子类四、线程池五、Semaphore六、CountDownLatch 前言 JUC&#xff08;Java.util.concurrent&#xff09;是 Java 标准库中的一个包&#xff0c;它提供了一组并发编程工具&#xff0c;本篇文章就介绍几组常见的 JUC 组…
阅读更多...
win10自带wifi共享功能

win10自带wifi共享功能

1、按下【wini】组合键打开windows设置&#xff0c;点击【网络和internet】&#xff1b; 2、按照下图&#xff0c;打开个移动热点&#xff0c;设置名称、密码。
阅读更多...
Blender--》页面布局及基本操作讲解

Blender--》页面布局及基本操作讲解

接下来我会在three.js专栏中分享关于3D建模知识的文章&#xff0c;如果学习three朋友并且想了解和学习3D建模&#xff0c;欢迎关注本专栏&#xff0c;关于这款3D建模软件blender的安装&#xff0c;我在前面的文章已经讲解过了&#xff0c;如果不了解的朋友可以去考考古&#xf…
阅读更多...
DeepinV20安装MSJDK17

DeepinV20安装MSJDK17

装什么版本的JDK https://learn.microsoft.com/zh-cn/java/openjdk/download#openjdk-17 通常来讲&#xff0c;选择最适应自己应用程序的版本&#xff0c;例如最新开发的程序基本需要运行在jdk17了&#xff0c;又或者前几年的java程序基本都是jdk11,再旧一点的jdk8。尽可能选…
阅读更多...
【C++深入浅出】类和对象中篇（六种默认成员函数、运算符重载）

【C++深入浅出】类和对象中篇（六种默认成员函数、运算符重载）

目录 一. 前言 二. 默认成员函数 三. 构造函数 3.1 概念 3.2 特性 四. 析构函数 4.1 概念 4.2 特性 五. 拷贝构造函数 5.1 概念 5.2 特性 六. 运算符重载 6.1 引入 6.2 概念 6.3 注意事项 6.4 重载示例 6.5 赋值运算符重载 6.6 前置和后置运算符重载 七. c…
阅读更多...
【Rust日报】2023-09-07 Servo 项目将加入欧洲 Linux 基金会

【Rust日报】2023-09-07 Servo 项目将加入欧洲 Linux 基金会

Servo 项目将加入欧洲 Linux 基金会 Servo 项目由 Mozilla Research 于 2012 年创建&#xff0c;是除编译器本身之外的首个主要 Rust 代码库&#xff0c;自此成为实验性网络引擎设计的标志。Servo 的主要组件已被集成到 Firefox 网络浏览器中&#xff0c;其若干解析器和其他底层…
阅读更多...
渗透测试基础之永恒之蓝漏洞复现

渗透测试基础之永恒之蓝漏洞复现

渗透测试MS17-010(永恒之蓝)的漏洞复现 目录 渗透测试MS17-010(永恒之蓝)的漏洞复现 目录 前言 思维导图 1,渗透测试 1,1,什么是渗透测试? 1.2,渗透测试的分类: 1.3,渗透测试的流程 1.3.1,前期交互 1.3.2,情报收集 1.3.3,威胁建模 1.3.4,漏洞分析 1.3.5,漏洞验…
阅读更多...
软件设计模式(五)：代理模式

软件设计模式(五)：代理模式

前言 代理模式是软件设计模式的重中之重&#xff0c;代理模式在实际应用比较多&#xff0c;比如Spring框架中的AOP。在这篇文章中荔枝将会梳理有关静态代理、动态代理的区别以及两种实现动态代理模式的方式。希望能对有需要的小伙伴有帮助~~~ 文章目录 前言 一、静态代理 二…
阅读更多...
自定义Dynamics 365实施和发布业务解决方案 - 1. 准备工作

自定义Dynamics 365实施和发布业务解决方案 - 1. 准备工作

在当前的商业世界中，竞争每时每刻都在加剧每个企业都必须找到在竞争中保持领先的直观方法。其中之一企业面临的主要挑战是在以便为客户提供更好的服务。在这样一个竞争激烈、要求苛刻的时代环境中，对客户关系管理软件的需求是正在增加。 Dynamics 365的CE功能强大且适应性强…
阅读更多...
使用JS实现一个简单的观察者模式（Observer）

使用JS实现一个简单的观察者模式（Observer）

聚沙成塔每天进步一点点 ⭐ 专栏简介⭐ 手撸Observer⭐ 写在最后 ⭐ 专栏简介 前端入门之旅&#xff1a;探索Web开发的奇妙世界 记得点击上方或者右侧链接订阅本专栏哦 几何带你启航前端之旅 欢迎来到前端入门之旅&#xff01;这个专栏是为那些对Web开发感兴趣、刚刚踏入前端领…
阅读更多...
MySQL数据库——存储引擎（1）-MySQL体系结构、存储引擎简介

MySQL数据库——存储引擎（1）-MySQL体系结构、存储引擎简介

目录 MySQL体系结构 连接层 服务层 引擎层 存储层 存储引擎简介 概念 语句 演示 下面开始学习进阶篇的第一个内容——存储引擎 分为四点学习&#xff1a; MySQL体系结构存储引擎简介存储引擎特点存储引擎选择 MySQL体系结构 连接层 最上层是一些客户端和链接服务&am…
阅读更多...
小米和金山集团董事长雷军访问武汉：加大投资力度，深化务实合作

小米和金山集团董事长雷军访问武汉：加大投资力度，深化务实合作

小米集团创始人雷军一行在9月6日到访了武汉&#xff0c;受到了当地政府的热情欢迎。武汉方面表示&#xff0c;小米、金山集团作为全球知名的企业集团&#xff0c;与武汉有着良好合作基础。未来&#xff0c;武汉希望小米、金山集团持续深耕武汉&#xff0c;加大投资力度&#xf…
阅读更多...
主页整理：8月1日---9月10日

主页整理：8月1日---9月10日

目录 8月1日17点 8月1日20点 8月3日13点 8月3日18点 8月15日19点 8月28日9点 8月28日18点 8月29日8点 8月29日9点 9月2日21点 9月5日17点 9月9日18点 9月10日7点 粉丝变化数 8月1日17点 8月1日20点 8月3日13点 8月3日18点 8月15日19点 8月28日9点 8月28日18点…
阅读更多...
Element-ui container常见布局

Element-ui container常见布局

1、header\main布局 <template> <div> <el-container> <el-header>Header</el-header> <el-main>Main</el-main> </el-container> </div> </template> <style> .el-header { …
阅读更多...
日常开发小汇总（3）js类型判断

日常开发小汇总（3）js类型判断

1.typeof 能判断出字符串、数字、方法和undefined&#xff0c;array、null、object判断不出 let num 1;let str "x";let fn function user(){}let arr [1,2]let obj {name:"zhangs"}let und;let nul null;console.log(typeof num) //numberconsole.l…
阅读更多...
深度、广度优先遍历（邻接表）

深度、广度优先遍历（邻接表）

#include<stdio.h> #include<stdlib.h> #include<iostream> #include<queue> #define MAXVEX 20 typedef char VertexType; using namespace std;//边表结点 typedef struct EdgeNode{int adjvex;struct EdgeNode *next; }EdgeNode;//顶点结点 typedef…
阅读更多...
Spring Cloud：构建微服务的最佳实践

Spring Cloud：构建微服务的最佳实践

&#x1f337;&#x1f341; 博主猫头虎&#xff08;&#x1f405;&#x1f43e;&#xff09;带您 Go to New World✨&#x1f341; &#x1f984; 博客首页——&#x1f405;&#x1f43e;猫头虎的博客&#x1f390; &#x1f433; 《面试题大全专栏》 &#x1f995; 文章图文…
阅读更多...
手机木马远程控制复现

手机木马远程控制复现

目录 目录 前言 系列文章列表 渗透测试基础之永恒之蓝漏洞复现http://t.csdn.cn/EsMu2 思维导图 1&#xff0c;实验涉及复现环境 2,Android模拟器环境配置 2.1,首先从官网上下载雷电模拟器 2.2,安装雷电模拟器 2.3, 对模拟器网络进行配置 2.3.1,为什么要进行配置…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023