ArduPilot开源代码之AP_OpticalFlow_UPFLOW

lida2003

已于 2024-07-11 06:35:54 修改

阅读量470

点赞数 4

分类专栏： ArduPilot 文章标签： ardupilot 开源 c++

于 2024-07-10 19:32:00 首次发布

本文链接：https://blog.csdn.net/lida2003/article/details/140332480

版权

ArduPilot 专栏收录该内容

157 篇文章 116 订阅

订阅专栏

ArduPilot开源代码之AP_OpticalFlow_UPFLOW

1. 源由
2. Library设计
3. 重要例程
4. 总结
5. 参考资料

1. 源由

AP_OpticalFlow_UPFLOW也是一种光流计，与前面AP_OpticalFlow_CXOF类似。

而且其波特率也是19200 bps，采用同样的pulling方式。唯一的差别就是两者帧格式略有差异。

2. Library设计

设备相关的重要函数：

init
update
detect

class AP_OpticalFlow_UPFLOW : public OpticalFlow_backend
{
public:
    /// 构造函数
    AP_OpticalFlow_UPFLOW(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *uart);

    // 初始化传感器
    void init() override;

    // 从传感器读取最新的值并填充 x, y 和总量
    void update(void) override;

    // 检测传感器是否可用
    static AP_OpticalFlow_UPFLOW *detect(AP_OpticalFlow &_frontend);

private:
    // 数据结构：光流数据
    struct PACKED UpixelsOpticalFlow {
        int16_t flow_x_integral;        // 单位：10^-4 弧度，乘以 10^-4 得到弧度值
        int16_t flow_y_integral;        // 单位：10^-4 弧度，乘以 10^-4 得到弧度值
        uint16_t integration_timespan;  // 时间间隔，单位：微秒
        uint16_t ground_distance;       // 保留字段，总是为 999
        uint8_t quality;                // 数据质量，0 表示无效，245 表示有效
        uint8_t version;                // 版本号
    };

    AP_HAL::UARTDriver *uart;               // 连接到光流传感器的 UART
    struct UpixelsOpticalFlow updata;       // 接收的数据结构
    uint16_t recv_count;                    // 接收到的字节数
    uint8_t sum;                            // 校验和
    Vector2f gyro_sum;                      // 自上次从光流传感器接收到数据以来陀螺仪传感器值的总和
    uint16_t gyro_sum_count;                // 陀螺仪传感器值的数量
};

3. 重要例程

3.1 AP_OpticalFlow_UPFLOW::init

模块默认波特率19200 bps。

// initialise the sensor
void AP_OpticalFlow_UPFLOW::init()
{
    // sanity check uart
    if (uart == nullptr) {
        return;
    }
    // open serial port with baud rate of 19200
    uart->begin(19200);
}

3.2 AP_OpticalFlow_UPFLOW::update

串口数据采用pull的方式读取，相对来说更占用CPU资源。

[AP_OpticalFlow_MSP](https://blog.csdn.net/lida2003/article/details/140328944)采用Interrupt方式比较节省资源，并且Matek光流计采用了115200 bps的波特率，速度更快，暂用CPU的时间更短。

update()
├── 检查UART是否有效
│   └── if (uart == nullptr)
│       └── return
├── 记录陀螺仪数值
│   └── if (gyro_sum_count >= 1000)
│       ├── gyro_sum.zero()  // 将陀螺仪累积值清零
│       └── gyro_sum_count = 0  // 重置累积计数
│   └── const Vector3f& gyro = AP::ahrs().get_gyro()  // 获取陀螺仪数据
│   └── gyro_sum.x += gyro.x  // 累积陀螺仪x轴数据
│   └── gyro_sum.y += gyro.y  // 累积陀螺仪y轴数据
│   └── gyro_sum_count++  // 累积计数加一
├── 读取串口数据
│   └── uint32_t nbytes = MIN(uart->available(), 1024u)  // 获取可用数据字节数
│   └── while (nbytes-- > 0)  // 循环读取每个字节数据
│       └── int16_t r = uart->read()  // 读取一个字节数据
│       └── if (r < 0)
│           └── break
│       └── uint8_t c = (uint8_t)r  // 转换为无符号字节
│       └── if (recv_count == 0)
│           └── if (c == UPFLOW_HEADER0)
│               └── recv_count++  // 接收计数加一
│               └── sum = 0  // 校验和清零
│       └── else if (recv_count == 1)
│           └── if (c != UPFLOW_HEADER1)
│               └── recv_count = 0  // 重置接收计数
│           └── else
│               └── recv_count++  // 接收计数加一
│       └── else if (recv_count < 12)
│           └── ((uint8_t*)&updata)[recv_count - 2] = c  // 存储接收到的数据
│           └── sum ^= c  // 更新校验和
│           └── recv_count++  // 接收计数加一
│       └── else if (recv_count == 12)
│           └── if (sum != c)
│               └── recv_count = 0  // 校验和错误，重置接收计数
│           └── else
│               └── recv_count++  // 接收计数加一
│       └── else
│           └── if (c == UPFLOW_FOOTER)
│               └── phrased = true  // 标记解析完成
│           └── recv_count = 0  // 重置接收计数
├── 如果没有有效数据，则返回
│   └── if (phrased == false)
│       └── return
├── 处理接收到的数据
│   └── struct AP_OpticalFlow::OpticalFlow_state state {}  // 定义光流状态结构体
│   └── state.surface_quality = updata.quality  // 设置表面质量
│   └── float dt = updata.integration_timespan * 1.0e-6  // 计算时间间隔
│   └── if (is_positive(dt) && (dt < UPFLOW_TIMEOUT_SEC))  // 如果时间间隔有效
│       ├── 计算流速数值
│       ├── const Vector2f flowScaler = _flowScaler()  // 获取流速缩放器
│       ├── Vector2f flowScaleFactor = Vector2f(1.0f, 1.0f) + flowScaler * 0.001f  // 计算流速缩放因子
│       ├── state.flowRate = Vector2f(-updata.flow_x_integral * flowScaleFactor.x, -updata.flow_y_integral * flowScaleFactor.y)  // 计算流速
│       ├── state.flowRate *= UPFLOW_PIXEL_SCALING / dt  // 根据时间间隔调整流速
│       ├── state.bodyRate = Vector2f(gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count)  // 计算体速度
│       └── _applyYaw(state.flowRate)  // 应用偏航角修正流速
│   └── else
│       └── state.flowRate.zero()  // 时间间隔无效时流速置零
│       └── state.bodyRate.zero()  // 时间间隔无效时体速度置零
└── 更新前端状态
    └── _update_frontend(state)  // 更新前端状态信息
└── 重置陀螺仪累积值
    └── gyro_sum.zero()  // 陀螺仪累积值清零
    └── gyro_sum_count = 0  // 重置陀螺仪累积计数

3.3 AP_OpticalFlow_UPFLOW::detect

AP_OpticalFlow初始化的内容直接放到了detect函数实现。

// detect the device
AP_OpticalFlow_UPFLOW *AP_OpticalFlow_UPFLOW::detect(AP_OpticalFlow &_frontend)
{
    AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
    if (serial_manager == nullptr) {
        return nullptr;
    }

    // look for first serial driver with protocol defined as OpticalFlow
    AP_HAL::UARTDriver *uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_OpticalFlow, 0);
    if (uart == nullptr) {
        return nullptr;
    }

    // we have found a serial port so use it
    AP_OpticalFlow_UPFLOW *sensor = new AP_OpticalFlow_UPFLOW(_frontend, uart);
    return sensor;
}