lenn/ts-qt
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: integrate tactile stream decoding

Browse Source
This commit is contained in:
lenn
2025-10-24 17:15:53 +08:00
parent d819d40fe1
commit 1f65ba0114
32 changed files with 1284 additions and 5400 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
components/charts/heatmap.cc
View File

@@ -5,12 +5,8 @@
#include "heatmap.hh"
#include "heatmap.impl.hh"
#include "qcustomplot/qcustomplot.h"
#include <qcolor.h>
#include <qcolormap.h>
#include <qcontainerfwd.h>
#include <qdebug.h>
#include <qnamespace.h>
#include <qobject.h>
#include <qsize.h>
using namespace creeper::plot_widget::internal;
@@ -21,32 +17,30 @@ BasicPlot::BasicPlot() : pimpl{std::make_unique<Impl>(*this)} {
setBackground(Qt::transparent);
}
BasicPlot::~BasicPlot() {}
BasicPlot::~BasicPlot() = default;
void BasicPlot::set_matrix_size(const QSize& size) {
qDebug() << "set matrix size" << size;
void BasicPlot::set_matrix_size(const QSize& size)const {
pimpl->set_matrix_size(size);
}
void BasicPlot::set_matrix_size(const int& w, const int& h) {
void BasicPlot::set_matrix_size(const int& w, const int& h)const {
QSize size(w, h);
qDebug() << "set matrix size" << size;
pimpl->set_matrix_size(size);
}
void BasicPlot::set_color_gradient_range(const double& min, const double& max) {
void BasicPlot::set_color_gradient_range(const double& min, const double& max)const {
pimpl->set_color_gradient_range(min, max);
}
void BasicPlot::set_xlabel_text(const QString& text) {
void BasicPlot::set_xlabel_text(const QString& text)const {
pimpl->set_xlabel_text(text);
}
void BasicPlot::set_ylabel_text(const QString& text) {
void BasicPlot::set_ylabel_text(const QString& text)const {
pimpl->set_ylabel_text(text);
}
void BasicPlot::load_theme_manager(ThemeManager& mgr) {
void BasicPlot::load_theme_manager(ThemeManager& mgr)const {
pimpl->load_theme_manager(mgr);
}
@@ -54,8 +48,7 @@ QSize BasicPlot::get_matrix_size() const {
return pimpl->get_matrix_size();
}
void BasicPlot::set_data(const QVector<PointData>& data) {
qDebug() << "set data" << data.size();
void BasicPlot::set_data(const QVector<PointData>& data)const {
pimpl->set_data(data);
}
@@ -63,7 +56,7 @@ bool BasicPlot::is_initialized() const {
return pimpl->is_plot_initialized();
}
void BasicPlot::init_plot() {
void BasicPlot::init_plot()const {
pimpl->initialize_plot();
}
@@ -75,17 +68,17 @@ void BasicPlot::paintEvent(QPaintEvent* event) {
QCustomPlot::paintEvent(event);
}
void BasicPlot::dataChanged(const QVector<PointData>& map) {
void BasicPlot::dataChanged(const QVector<PointData>& map)const {
set_data(map);
// emit dataChanged(map);
}
void BasicPlot::dataRangeChanged(const double& min, const double& max) {
void BasicPlot::dataRangeChanged(const double& min, const double& max)const {
set_color_gradient_range(min, max);
// emit dataRangeChanged(min, max);
}
void BasicPlot::update_dynamic_heatmap(const QVector<PointData>& map) {
void BasicPlot::update_dynamic_heatmap(const QVector<PointData>& map)const {
set_data(map);
}

22
components/charts/heatmap.hh
View File

@@ -35,21 +35,21 @@ public:
// BasicPlot(const BasicPlot&) = delete;
// BasicPlot& operator=(const BasicPlot&) = delete;
void init_plot();
void load_theme_manager(ThemeManager&);
void set_xlabel_text(const QString&);
void set_ylabel_text(const QString&);
void set_matrix_size(const QSize&);
void set_matrix_size(const int& w, const int& h);
void set_data(const QVector<PointData>& data);
void set_color_gradient_range(const double& min, const double& max);
void init_plot()const;
void load_theme_manager(ThemeManager&)const;
void set_xlabel_text(const QString&)const;
void set_ylabel_text(const QString&)const;
void set_matrix_size(const QSize&)const;
void set_matrix_size(const int& w, const int& h)const;
void set_data(const QVector<PointData>& data)const;
void set_color_gradient_range(const double& min, const double& max)const;
QSize get_matrix_size() const;
bool is_initialized() const;
public slots:
void update_dynamic_heatmap(const QVector<PointData>& map);
void dataChanged(const QVector<PointData>& map);
void dataRangeChanged(const double& min, const double& max);
void update_dynamic_heatmap(const QVector<PointData>& map)const;
void dataChanged(const QVector<PointData>& map)const;
void dataRangeChanged(const double& min, const double& max)const;
protected:
void paintEvent(QPaintEvent*) override;

1
components/charts/heatmap.impl.hh
View File

@@ -75,7 +75,6 @@ public:
// 创建颜色映射
QCPColorMap* cpmp = new QCPColorMap(self.xAxis, self.yAxis);
qDebug() << "initialize_plot() size:" << matrix_size;
cpmp->data()->setSize(matrix_size.width(), matrix_size.height());
cpmp->data()->setRange(QCPRange(0.5, matrix_size.width() - 0.5),
QCPRange(0.5, matrix_size.height() - 0.5));

226
components/comment/cpdecoder.cc
View File

@@ -1,226 +0,0 @@
//
// FFmpeg 风格编解码器注册表与上下文管理实现（中文注释版）。
// 注意：此实现大量使用 std::vector、std::optional 等现代 C++ 容器与 RAII 思想。
//
#include "cpdecoder.hh"
#include <algorithm> // 提供 std::find_if 等算法，用于查找与遍历
#include <mutex> // std::mutex + std::lock_guard，保证注册表线程安全
namespace ffmsep {
namespace { // 匿名命名空间：限制辅助函数和静态变量作用域在当前编译单元
std::vector<const CPCodec*>& codec_registry() {
static std::vector<const CPCodec*> registry; // 使用静态局部变量 + vector 保留所有注册的编解码器
return registry;
}
std::mutex& registry_mutex() {
static std::mutex m; // 构建一个全局互斥量，用于守护注册表操作
return m;
}
void attach_codec(CPCodecContext* ctx, const CPCodec* codec) {
if (!ctx) {
return;
}
ctx->codec = codec;
if (!codec) {
ctx->codec_type = CPMediaType::Unknown;
ctx->priv_data = nullptr;
ctx->release_priv_storage();
return;
}
ctx->codec_type = codec->type;
ctx->priv_data = ctx->ensure_priv_storage(codec->priv_data_size);
}
bool codec_name_equals(const CPCodec* codec, std::string_view name) {
if (!codec || !codec->name) {
return false;
}
return std::string_view(codec->name) == name; // std::string_view 避免额外拷贝
}
} // namespace
void* CPCodecContext::ensure_priv_storage(std::size_t size) {
// 将 vector 当作动态缓冲区，确保大小满足编解码器私有数据需求
if (size == 0U) {
priv_storage.clear();
priv_data = nullptr;
return nullptr;
}
if (priv_storage.size() != size) {
priv_storage.assign(size, static_cast<std::uint8_t>(0));
}
priv_data = priv_storage.data();
return priv_data;
}
void CPCodecContext::release_priv_storage() noexcept {
// 与 ensure 配套，释放时直接清空 vector
priv_storage.clear();
priv_data = nullptr;
}
// 将单个编解码器指针注册到全局列表，重复注册时作安全检查。
void cpcodec_register(const CPCodec* codec) {
if (!codec || !codec->name) {
return;
}
std::lock_guard<std::mutex> lock(registry_mutex());
auto& reg = codec_registry();
auto already = std::find(reg.begin(), reg.end(), codec);
if (already != reg.end()) {
return;
}
auto same_id = std::find_if(reg.begin(), reg.end(), [codec](const CPCodec* entry) {
return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknown;
});
if (same_id != reg.end()) {
*same_id = codec; // 如果 ID 已存在，则替换为新实现，以最新注册者为准
return;
}
reg.push_back(codec);
}
// 批量注册函数：利用 C++11 std::initializer_list 便捷传入多个指针。
void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs) {
for (const CPCodec* codec : codecs) {
cpcodec_register(codec);
}
}
const CPCodec* cpcodec_find_decoder(CPCodecID id) {
std::lock_guard<std::mutex> lock(registry_mutex());
const auto& reg = codec_registry();
auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec) {
return codec && codec->id == id;
});
return it == reg.end() ? nullptr : *it; // 使用可空返回值：找不到时返回 nullptr
}
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name) {
std::lock_guard<std::mutex> lock(registry_mutex());
const auto& reg = codec_registry();
auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec) {
return codec_name_equals(codec, name);
});
return it == reg.end() ? nullptr : *it;
}
std::vector<const CPCodec*> cpcodec_list_codecs() {
std::lock_guard<std::mutex> lock(registry_mutex());
return codec_registry(); // 返回当前注册表的浅拷贝
}
// 分配一个新的上下文，并可选绑定已有编解码器。
CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec) {
auto* ctx = new CPCodecContext();
if (codec) {
attach_codec(ctx, codec);
}
return ctx;
}
// 打开上下文：允许调用方在此时指定（或替换）具体编解码器。
int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) {
if (!ctx) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (ctx->is_open) {
return CP_ERROR_INVALID_STATE;
}
if (codec) {
attach_codec(ctx, codec);
}
if (!ctx->codec) {
return CP_ERROR_INVALID_ARGUMENT;
}
ctx->is_open = true;
if (ctx->codec->init) {
int rc = ctx->codec->init(ctx);
if (rc < 0) {
ctx->is_open = false;
if (ctx->codec->close) {
ctx->codec->close(ctx); // 初始化失败时调用 close 进行善后
}
return rc;
}
}
return CP_SUCCESS;
}
// 关闭上下文：调用编解码器收尾逻辑并释放私有状态。
int cpcodec_close(CPCodecContext* ctx) {
if (!ctx) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (!ctx->is_open) {
return CP_SUCCESS;
}
if (ctx->codec && ctx->codec->close) {
ctx->codec->close(ctx);
}
ctx->is_open = false;
ctx->release_priv_storage();
ctx->codec_type = CPMediaType::Unknown;
ctx->codec = nullptr;
ctx->priv_data = nullptr;
return CP_SUCCESS;
}
// 释放上下文指针，防止悬挂引用。
void cpcodec_free_context(CPCodecContext** ctx) {
if (!ctx || !*ctx) {
return;
}
cpcodec_close(*ctx);
delete *ctx;
*ctx = nullptr;
}
// 输入侧 API：推送一帧串口数据，内部队列通常会缓冲。
int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) {
if (!ctx || !packet) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (!ctx->is_open || !ctx->codec) {
return CP_ERROR_NOT_OPEN;
}
if (!ctx->codec->send_packet) {
return CP_ERROR_INVALID_STATE;
}
return ctx->codec->send_packet(ctx, *packet);
}
// 输出侧 API：尝试从编解码器读取一帧解码结果。
int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) {
if (!ctx || !frame) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (!ctx->is_open || !ctx->codec) {
return CP_ERROR_NOT_OPEN;
}
if (!ctx->codec->receive_frame) {
return CP_ERROR_INVALID_STATE;
}
return ctx->codec->receive_frame(ctx, *frame);
}
} // namespace ffmsep

141
components/comment/cpdecoder.hh
View File

@@ -1,141 +0,0 @@
//
// FFmpeg 风格的串口解码工具核心定义（带详细中文注释）。
// 说明：本文件使用了 C++17 引入的 std::optional（可选值容器）与 [[nodiscard]] 属性，
// 以及 C++11 的 std::initializer_list、基于 enum class 的强类型枚举，用于提高类型安全。
//
#pragma once
#include <cstdint> // 固定宽度整数类型，便于协议字段与位宽一致
#include <cstddef>
#include <mutex> // 互斥锁，用于注册表线程安全
#include <optional> // C++17 可选类型，用来表示可能为空的查找结果
#include <string>
#include <string_view> // C++17 字符串视图，避免不必要的拷贝
#include <vector>
#include <initializer_list> // C++11 可初始化列表，支持批量注册编解码器
namespace ffmsep { // 命名空间隔离所有与串口编解码相关的类型
// 错误码定义：参考 FFmpeg 的返回值约定，所有负数表示异常。
inline constexpr int CP_SUCCESS = 0; // 成功
inline constexpr int CP_ERROR_EOF = -1; // 流结束（End Of File）
inline constexpr int CP_ERROR_EAGAIN = -2; // 数据暂不可用，稍后重试
inline constexpr int CP_ERROR_NOT_OPEN = -3; // 编解码上下文尚未打开
inline constexpr int CP_ERROR_INVALID_STATE = -4; // 当前状态不允许该操作
inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;// 传入参数不合法
// enum class 使用 C++11 强类型枚举，避免隐式转换导致的错误。
enum class CPMediaType : std::uint8_t {
Unknown = 0, // 未知类型：默认值
Data, // 数据流（例如串口数据）
Audio, // 音频流（预留扩展）
Video // 视频流（预留扩展）
};
// 编解码器 ID，用于在注册表中快速定位具体实现。
enum class CPCodecID : std::uint32_t {
Unknown = 0, // 未知或未设置
Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识
};
// CPPacket 表示输入的“帧包”，与 FFmpeg 中 AVPacket 的思想类似。
struct CPPacket {
std::vector<std::uint8_t> payload; // 有效载荷：串口原始数据
std::int64_t pts = 0; // 显示时间戳（presentation timestamp）
std::int64_t dts = 0; // 解码时间戳（decode timestamp）
bool end_of_stream = false; // 标记此包是否为流结尾
bool flush = false; // 是否请求刷新内部状态（例如重置缓冲）
CPPacket() = default;
CPPacket(std::vector<std::uint8_t> data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept
: payload(std::move(data)), pts(pts_value), dts(dts_value) {}
[[nodiscard]] bool empty() const noexcept { return payload.empty(); } // [[nodiscard]] 防止忽略返回值
};
// CPFrame 表示解码后的数据帧，对应业务层可消费的实体。
struct CPFrame {
std::vector<std::uint8_t> data; // 解码后的数据内容
std::int64_t pts = 0; // 与输入包对应的时间戳
bool key_frame = false; // 是否为关键帧（例如起始帧）
bool valid = false; // 是否包含有效数据
void reset() noexcept {
data.clear();
pts = 0;
key_frame = false;
valid = false;
}
};
struct CPCodecContext;
// CPCodec 用函数指针描述具体编解码器的行为，等价于 FFmpeg 中的 AVCodec。
struct CPCodec {
using InitFn = int (*)(CPCodecContext*); // 初始化回调
using CloseFn = void (*)(CPCodecContext*); // 关闭回调
using SendPacketFn = int (*)(CPCodecContext*, const CPPacket&); // 发送输入包
using ReceiveFrameFn = int (*)(CPCodecContext*, CPFrame&); // 拉取输出帧
const char* name = nullptr;
const char* long_name = nullptr;
CPMediaType type = CPMediaType::Unknown;
CPCodecID id = CPCodecID::Unknown;
std::size_t priv_data_size = 0;
InitFn init = nullptr;
CloseFn close = nullptr;
SendPacketFn send_packet = nullptr;
ReceiveFrameFn receive_frame = nullptr;
};
struct CPCodecContext {
const CPCodec* codec = nullptr; // 指向当前使用的编解码器描述
void* priv_data = nullptr; // 指向编解码器私有状态（大小由 priv_data_size 控制）
CPMediaType codec_type = CPMediaType::Unknown; // 保存媒体类型，便于外部查询
bool is_open = false; // 是否已经成功调用 open
void clear() noexcept {
codec = nullptr;
priv_data = nullptr;
codec_type = CPMediaType::Unknown;
is_open = false;
priv_storage.clear();
}
void* ensure_priv_storage(std::size_t size); // 确保私有存储空间足够，不足时重新分配
void release_priv_storage() noexcept; // 释放私有存储
template <typename T>
[[nodiscard]] T* priv_as() noexcept {
return static_cast<T*>(priv_data);
}
template <typename T>
[[nodiscard]] const T* priv_as() const noexcept {
return static_cast<const T*>(priv_data);
}
private:
std::vector<std::uint8_t> priv_storage; // 私有缓冲区，使用 std::vector 管理生命周期
friend CPCodecContext* cpcodec_alloc_context3(const CPCodec*);
friend int cpcodec_open2(CPCodecContext*, const CPCodec*);
friend int cpcodec_close(CPCodecContext*);
};
// 注册接口：允许外部模块将编解码器加入全局列表。
void cpcodec_register(const CPCodec* codec);
void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs);
const CPCodec* cpcodec_find_decoder(CPCodecID id);
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
std::vector<const CPCodec*> cpcodec_list_codecs();
CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec); // 分配上下文，关联指定编解码器
int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec = nullptr); // 打开上下文，可在此处指定或替换编解码器
int cpcodec_close(CPCodecContext* ctx); // 关闭上下文并释放资源
void cpcodec_free_context(CPCodecContext** ctx); // 释放上下文指针
int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet); // 推送一份待解码的数据包
int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame); // 拉取解码出来的帧
} // namespace ffmsep

56
components/comment/tactile/tacdec.h
View File

@@ -1,56 +0,0 @@
//
// 触觉传感器串口协议高层解析工具（中文注释版）。
// 注意：文件使用了 C++17 的 std::optional 与 inline constexpr，
// 可提供编译期常量与安全的可空返回值。
//
#pragma once
#include "../cpdecoder.hh"
#include <cstdint> // 协议字段均以固定宽度字节表示
#include <optional> // std::optional：可能解析失败时返回空
#include <vector>
namespace ffmsep::tactile {
inline constexpr std::uint8_t kStartByte = 0x3A; // 帧起始符（冒号）
inline constexpr std::uint8_t kEndByteFirst = 0x0D; // 帧结束符（回车）
inline constexpr std::uint8_t kEndByteSecond = 0x0A; // 帧结束符（换行）
// 功能码枚举：使用 enum class 提升类型安全，避免与其他数值混用。
enum class FunctionCode : std::uint8_t {
Unknown = 0x00, // 未知功能，解析失败或未初始化
ReadMatrix = 0x01, // 读取整块矩阵 AD 值
ReadSingle = 0x02, // 读取单点 AD 值
ReadTemperature = 0x03,// 读取温度数据
SetDeviceId = 0x51, // 修改设备编号
SetMatrixSize = 0x52, // 修改矩阵尺寸（长边/短边）
CalibrationMode = 0x53 // 进入校准模式
};
struct MatrixSize {
std::uint8_t long_edge = 0; // 矩阵长边尺寸
std::uint8_t short_edge = 0; // 矩阵短边尺寸
};
struct TactileFrame {
std::uint8_t device_address = 0; // 设备地址（1~255）
FunctionCode function = FunctionCode::Unknown; // 功能码
std::uint8_t data_length = 0; // 数据域长度
std::vector<std::uint8_t> payload; // 数据域内容（按协议解析）
};
// 将底层 CPFrame 解析为结构化的 TactileFrame。
std::optional<TactileFrame> parse_frame(const CPFrame& frame);
// 将数据域按小端 16 位压力值数组解析。
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame);
// 解析矩阵尺寸（用于读/写矩阵尺寸的命令）。
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
// 解析矩阵坐标（复用尺寸结构，分别表示长边/短边索引）。
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame);
const CPCodec* tactile_codec();
void register_tactile_codec();
} // namespace ffmsep::tactile

297
components/comment/tactile/tecdec.cc
View File

@@ -1,297 +0,0 @@
//
// 触觉传感器串口协议解码器实现（中文注释版）。
// 使用要点：
// 1. 通过循环缓冲累积串口字节，按起始/结束符重组完整帧；
// 2. 使用 C++17 std::vector/std::optional 管理内存与返回值；
// 3. placement new（<new> 头文件）在已有内存上构造上下文对象。
//
#include "tacdec.h"
#include <algorithm> // std::find 等算法，用于搜索起始符
#include <cstddef>
#include <cstdint>
#include <new> // placement new：在已分配内存上构造对象
namespace ffmsep::tactile { // 与头文件保持一致的命名空间层次
namespace { // 匿名命名空间，用于封装文件内部的工具常量与函数
constexpr std::size_t kMinimumFrameSize = 1 // start 起始符
+ 1 // address 设备地址
+ 1 // function 功能码
+ 1 // length 数据长度
+ 0 // payload 最短为 0
+ 2 // CRC 校验
+ 2; // end markers 结束符
constexpr std::uint16_t kCrcInitial = 0xFFFF; // CRC 初始值
constexpr std::uint16_t kCrcPolynomial = 0xA001; // CRC-16/MODBUS 多项式 (LSB-first)
// 解码器私有上下文，跟随 CPCodecContext 的 priv_data 生命周期。
struct TactileDecoderContext {
std::vector<std::uint8_t> fifo; // 环形缓冲，存储尚未解析的原始字节
bool end_of_stream = false; // 标记是否收到流结尾
std::int64_t next_pts = 0; // 输出帧的自增 pts
};
std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) {
std::uint16_t crc = kCrcInitial;
for (std::size_t i = 0; i < length; ++i) {
crc ^= static_cast<std::uint16_t>(data[i]);
for (int bit = 0; bit < 8; ++bit) {
if ((crc & 0x0001U) != 0U) {
crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial); // LSB 为 1 时异或多项式
} else {
crc = static_cast<std::uint16_t>(crc >> 1U); // 否则右移
}
}
}
return crc;
}
TactileDecoderContext* get_priv(CPCodecContext* ctx) {
return ctx ? ctx->priv_as<TactileDecoderContext>() : nullptr; // 使用模板封装的安全转换
}
int tactile_init(CPCodecContext* ctx) {
if (!ctx) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (!ctx->priv_data) {
ctx->ensure_priv_storage(sizeof(TactileDecoderContext));
}
auto* storage = static_cast<TactileDecoderContext*>(ctx->priv_data);
new (storage) TactileDecoderContext(); // placement new：在已分配的缓冲区中原地构造对象
return CP_SUCCESS;
}
void tactile_close(CPCodecContext* ctx) {
if (!ctx || !ctx->priv_data) {
return;
}
if (auto* priv = get_priv(ctx); priv != nullptr) {
priv->~TactileDecoderContext(); // 显式调用析构函数，释放内部 std::vector
}
}
int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) {
auto* priv = get_priv(ctx);
if (!priv) {
return CP_ERROR_INVALID_STATE;
}
if (packet.flush) {
priv->fifo.clear(); // flush: 清除所有缓存，回到初始状态
priv->end_of_stream = false;
priv->next_pts = 0;
}
if (!packet.payload.empty()) {
priv->fifo.insert(priv->fifo.end(), packet.payload.begin(), packet.payload.end()); // 拼接新字节
}
if (packet.end_of_stream) {
priv->end_of_stream = true; // 标记输入源已结束
}
return CP_SUCCESS;
}
std::size_t frame_length_from_payload(std::uint8_t payload_length) {
return 1U + 1U + 1U + 1U + payload_length + 2U + 2U; // 计算完整帧总长度
}
const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
return buf.empty() ? nullptr : buf.data();
}
int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
auto* priv = get_priv(ctx);
if (!priv) {
return CP_ERROR_INVALID_STATE;
}
auto& buf = priv->fifo;
while (true) {
if (buf.empty()) {
if (priv->end_of_stream) {
priv->end_of_stream = false;
return CP_ERROR_EOF; // 没有数据且流结束
}
return CP_ERROR_EAGAIN; // 告诉调用者需要更多数据
}
// Discard bytes until start byte is found.
auto start_it = std::find(buf.begin(), buf.end(), kStartByte);
if (start_it == buf.end()) {
buf.clear();
if (priv->end_of_stream) {
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
if (start_it != buf.begin()) {
buf.erase(buf.begin(), start_it); // 丢掉起始符前的噪声
}
if (buf.size() < kMinimumFrameSize) {
if (priv->end_of_stream) {
// Incomplete frame at end of stream: drop it and report EOF.
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN; // 帧尚未完整，继续等待
}
const std::uint8_t* data = buffer_data(buf);
const std::uint8_t address = data[1];
const std::uint8_t function = data[2];
const std::uint8_t payload_length = data[3];
const std::size_t total_frame_length = frame_length_from_payload(payload_length); // 根据长度字段推算完整帧尺寸
if (buf.size() < total_frame_length) {
if (priv->end_of_stream) {
// Not enough data before stream end: treat as EOF and drop buffer.
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
const std::size_t payload_offset = 4U;
const std::size_t crc_offset = payload_offset + payload_length;
const std::size_t end_offset = crc_offset + 2U; // CRC 后紧接 0x0D 0x0A
const std::uint8_t crc_lo = data[crc_offset];
const std::uint8_t crc_hi = data[crc_offset + 1U];
const std::uint16_t crc_value = static_cast<std::uint16_t>(crc_lo) |
static_cast<std::uint16_t>(crc_hi << 8U);
const std::uint8_t end_first = data[end_offset];
const std::uint8_t end_second = data[end_offset + 1U];
if (end_first != kEndByteFirst || end_second != kEndByteSecond) {
// Invalid end marker, drop start byte and retry.
buf.erase(buf.begin());
continue;
}
const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload
const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length);
if (computed_crc != crc_value) {
buf.erase(buf.begin()); // CRC 校验失败，丢弃该起始符并重新同步
continue;
}
(void)address; // 当前实现仅检查 CRC，不直接使用地址/功能码
(void)function;
frame.data.assign(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
frame.pts = priv->next_pts++;
frame.key_frame = true;
frame.valid = true;
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length)); // 移除已消费的内容
return CP_SUCCESS;
}
}
const CPCodec kTactileCodec {
"tactile_serial",
"Framed tactile sensor serial protocol decoder",
CPMediaType::Data,
CPCodecID::Tactile,
sizeof(TactileDecoderContext),
&tactile_init,
&tactile_close,
&tactile_send_packet,
&tactile_receive_frame
};
} // namespace
// 将底层 CPFrame 转换为协议专用的 TactileFrame，失败时返回 std::nullopt。
std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
return std::nullopt;
}
const auto* bytes = frame.data.data();
const std::size_t size = frame.data.size();
if (bytes[0] != kStartByte) {
return std::nullopt;
}
if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) {
return std::nullopt;
}
if (size < 4U) {
return std::nullopt;
}
const std::uint8_t length = bytes[3];
if (frame_length_from_payload(length) != size) {
return std::nullopt;
}
const std::uint8_t address = bytes[1];
const FunctionCode function = static_cast<FunctionCode>(bytes[2]);
const std::size_t payload_offset = 4U;
TactileFrame parsed{};
parsed.device_address = address;
parsed.function = function;
parsed.data_length = length;
parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + length);
return parsed;
}
// 将数据域按照小端排列的 16 位压力值序列解析为整数数组。
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
return {};
}
std::vector<std::uint16_t> values;
values.reserve(frame.payload.size() / 2U);
for (std::size_t idx = 0; idx + 1U < frame.payload.size(); idx += 2U) {
const std::uint16_t value = static_cast<std::uint16_t>(
static_cast<std::uint16_t>(frame.payload[idx]) |
static_cast<std::uint16_t>(frame.payload[idx + 1U] << 8U));
values.push_back(value);
}
return values;
}
// 解析矩阵尺寸载荷：两个字节分别为长边/短边。
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame) {
if (frame.payload.size() != 2U) {
return std::nullopt;
}
MatrixSize size{};
size.long_edge = frame.payload[0];
size.short_edge = frame.payload[1];
return size;
}
// 解析矩阵坐标：协议格式与尺寸一致，直接复用逻辑。
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame) {
return parse_matrix_size_payload(frame);
}
// 提供对外查询：返回触觉协议对应的 CPCodec 描述。
const CPCodec* tactile_codec() {
return &kTactileCodec;
}
// 将触觉协议编解码器注册到全局列表，供 cpcodec_find_decoder 使用。
void register_tactile_codec() {
cpcodec_register(&kTactileCodec);
}
} // namespace ffmsep::tactile

54
components/ffmsep/cpdecoder.cc
View File

@@ -1,14 +1,14 @@
//
// Core FFmpeg-style codec registry and decoding helpers.
//
#include "cpdecoder.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <cstddef>
#include <initializer_list>
#include <mutex>
#include <string_view>
#include <vector>
namespace ffmsep {
namespace {
std::vector<const CPCodec*>& codec_registry() {
@@ -28,7 +28,7 @@ void attach_codec(CPCodecContext* ctx, const CPCodec* codec) {
ctx->codec = codec;
if (!codec) {
ctx->codec_type = CPMediaType::Unknown;
ctx->codec_type = CPMediaType::Data;
ctx->priv_data = nullptr;
ctx->release_priv_storage();
return;
@@ -42,11 +42,12 @@ bool codec_name_equals(const CPCodec* codec, std::string_view name) {
if (!codec || !codec->name) {
return false;
}
return std::string_view(codec->name) == name;
}
} // namespace
using namespace ffmsep;
void* CPCodecContext::ensure_priv_storage(std::size_t size) {
if (size == 0U) {
priv_storage.clear();
@@ -65,7 +66,7 @@ void CPCodecContext::release_priv_storage() noexcept {
priv_data = nullptr;
}
void cpcodec_register(const CPCodec* codec) {
void cpcodec_register(const CPCodec *codec) {
if (!codec || !codec->name) {
return;
}
@@ -76,9 +77,9 @@ void cpcodec_register(const CPCodec* codec) {
if (already != reg.end()) {
return;
}
auto same_id = std::find_if(reg.begin(), reg.end(), [codec](const CPCodec* entry) {
return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknown;
return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknow;
});
if (same_id != reg.end()) {
*same_id = codec;
@@ -97,7 +98,7 @@ void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs) {
const CPCodec* cpcodec_find_decoder(CPCodecID id) {
std::lock_guard<std::mutex> lock(registry_mutex());
const auto& reg = codec_registry();
auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec) {
auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec){
return codec && codec->id == id;
});
return it == reg.end() ? nullptr : *it;
@@ -106,10 +107,11 @@ const CPCodec* cpcodec_find_decoder(CPCodecID id) {
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name) {
std::lock_guard<std::mutex> lock(registry_mutex());
const auto& reg = codec_registry();
auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec) {
auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec){
return codec_name_equals(codec, name);
});
return it == reg.end() ? nullptr : *it;
return it ==reg.end() ? nullptr : *it;
}
std::vector<const CPCodec*> cpcodec_list_codecs() {
@@ -117,15 +119,16 @@ std::vector<const CPCodec*> cpcodec_list_codecs() {
return codec_registry();
}
CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec) {
CPCodecContext* cpcodec_alloc_context(const CPCodec* codec) {
auto* ctx = new CPCodecContext();
if (codec) {
attach_codec(ctx, codec);
}
return ctx;
}
int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) {
int cpcodec_open(CPCodecContext* ctx, const CPCodec* codec) {
if (!ctx) {
return CP_ERROR_INVALID_ARGUMENT;
}
@@ -153,10 +156,11 @@ int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) {
return rc;
}
}
return CP_SUCCESS;
}
int cpcodec_close(CPCodecContext* ctx) {
int cpcodec_close(CPCodecContext *ctx) {
if (!ctx) {
return CP_ERROR_INVALID_ARGUMENT;
}
@@ -169,28 +173,33 @@ int cpcodec_close(CPCodecContext* ctx) {
ctx->codec->close(ctx);
}
if (ctx->codec && ctx->codec->close) {
ctx->codec->close(ctx);
}
ctx->is_open = false;
ctx->release_priv_storage();
ctx->codec_type = CPMediaType::Unknown;
ctx->codec_type = CPMediaType::Unknow;
ctx->codec = nullptr;
ctx->priv_data = nullptr;
return CP_SUCCESS;
}
void cpcodec_free_context(CPCodecContext** ctx) {
void cpcodec_free_context(CPCodecContext **ctx) {
if (!ctx || !*ctx) {
return;
}
cpcodec_close(*ctx);
delete *ctx;
*ctx = nullptr;
}
int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) {
int cpcodec_send_packet(CPCodecContext *ctx, const CPPacket *packet) {
if (!ctx || !packet) {
return CP_ERROR_INVALID_ARGUMENT;
}
if (!ctx->is_open || !ctx->codec) {
if (!ctx || !ctx->codec) {
return CP_ERROR_NOT_OPEN;
}
if (!ctx->codec->send_packet) {
@@ -199,7 +208,7 @@ int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) {
return ctx->codec->send_packet(ctx, *packet);
}
int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) {
int cpcodec_receive_frame(CPCodecContext *ctx, CPFrame *frame) {
if (!ctx || !frame) {
return CP_ERROR_INVALID_ARGUMENT;
}
@@ -211,5 +220,4 @@ int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) {
}
return ctx->codec->receive_frame(ctx, *frame);
}
} // namespace ffmsep
}

58
components/ffmsep/cpdecoder.hh
View File

@@ -1,9 +1,6 @@
//
// Simple FFmpeg-inspired serial decoding toolkit.
//
#pragma once
#include "components/ffmsep/cpdecoder.hh"
#include <cstdint>
#include <cstddef>
#include <mutex>
@@ -15,7 +12,6 @@
namespace ffmsep {
// Error codes loosely mirroring FFmpeg semantics.
inline constexpr int CP_SUCCESS = 0;
inline constexpr int CP_ERROR_EOF = -1;
inline constexpr int CP_ERROR_EAGAIN = -2;
@@ -24,15 +20,13 @@ inline constexpr int CP_ERROR_INVALID_STATE = -4;
inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;
enum class CPMediaType : std::uint8_t {
Unknown = 0,
Unknow = 0,
Data,
Audio,
Video
};
enum class CPCodecID : std::uint32_t {
Unknown = 0,
Tactile = 0x54514354u // 'T','Q','C','T' marker for tactile quick codec.
Unknow = 0,
Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识 Tactile Quick Codec Type
};
struct CPPacket {
@@ -46,7 +40,7 @@ struct CPPacket {
CPPacket(std::vector<std::uint8_t> data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept
: payload(std::move(data)), pts(pts_value), dts(dts_value) {}
[[nodiscard]] bool empty() const noexcept { return payload.empty(); }
[[nodiscard]] bool empty() const noexcept {return payload.empty();}
};
struct CPFrame {
@@ -57,24 +51,24 @@ struct CPFrame {
void reset() noexcept {
data.clear();
pts = 0;
key_frame = false;
valid = false;
pts = 0;
}
};
struct CPCodecContext;
struct CPCodec {
using InitFn = int (*)(CPCodecContext*);
using CloseFn = void (*)(CPCodecContext*);
using SendPacketFn = int (*)(CPCodecContext*, const CPPacket&);
using ReceiveFrameFn = int (*)(CPCodecContext*, CPFrame&);
using InitFn = int(*)(CPCodecContext*);
using CloseFn = void(*)(CPCodecContext*);
using SendPacketFn = int(*)(CPCodecContext*, const CPPacket&);
using ReceiveFrameFn = int(*)(CPCodecContext*, CPFrame&);
const char* name = nullptr;
const char* long_name = nullptr;
CPMediaType type = CPMediaType::Unknown;
CPCodecID id = CPCodecID::Unknown;
CPMediaType type = CPMediaType::Unknow;
CPCodecID id = CPCodecID::Unknow;
std::size_t priv_data_size = 0;
InitFn init = nullptr;
CloseFn close = nullptr;
@@ -85,13 +79,13 @@ struct CPCodec {
struct CPCodecContext {
const CPCodec* codec = nullptr;
void* priv_data = nullptr;
CPMediaType codec_type = CPMediaType::Unknown;
CPMediaType codec_type = CPMediaType::Unknow;
bool is_open = false;
void clear() noexcept {
codec = nullptr;
priv_data = nullptr;
codec_type = CPMediaType::Unknown;
codec_type = CPMediaType::Unknow;
is_open = false;
priv_storage.clear();
}
@@ -99,21 +93,20 @@ struct CPCodecContext {
void* ensure_priv_storage(std::size_t size);
void release_priv_storage() noexcept;
template <typename T>
template<typename T>
[[nodiscard]] T* priv_as() noexcept {
return static_cast<T*>(priv_data);
}
template <typename T>
template<typename T>
[[nodiscard]] const T* priv_as() const noexcept {
return static_cast<const T*>(priv_data);
}
private:
std::vector<std::uint8_t> priv_storage;
friend CPCodecContext* cpcodec_alloc_context3(const CPCodec*);
friend int cpcodec_open2(CPCodecContext*, const CPCodec*);
friend CPCodecContext* cpcodec_alloc_context(const CPCodec*);
friend int cpcodec_open(CPCodecContext*, const CPCodec*);
friend int cpcodec_close(CPCodecContext*);
};
@@ -123,11 +116,10 @@ const CPCodec* cpcodec_find_decoder(CPCodecID id);
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
std::vector<const CPCodec*> cpcodec_list_codecs();
CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec);
int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec = nullptr);
int cpcodec_close(CPCodecContext* ctx);
void cpcodec_free_context(CPCodecContext** ctx);
int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet);
int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame);
} // namespace ffmsep
CPCodecContext* cpcodec_alloc_context(const CPCodec* codec);
int cpcodec_open(CPCodecContext*, const CPCodec*);
int cpcodec_close(CPCodecContext*);
void cpcodec_free_context(CPCodecContext **ctx);
int cpcodec_send_packet(CPCodecContext*, const CPPacket*);
int cpcodec_receive_frame(CPCodecContext*, CPFrame*);
}

627
components/ffmsep/cpstream_core.cc Normal file
View File

@@ -0,0 +1,627 @@
#include "components/ffmsep/cpstream_core.hh"
#include <algorithm>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <cstddef>
#include <cstdint>
#include <deque>
#include <memory>
#include <mutex>
#include <optional>
#include <thread>
#include <utility>
#include <vector>
namespace ffmsep {
namespace {
constexpr auto kReaderIdleSleep = std::chrono::milliseconds(5);
constexpr auto kDecoderIdleSleep = std::chrono::milliseconds(1);
const CPCodec* resolve_requested_codec(const CPStreamConfig& config) {
if (!config.codec_name.empty()) {
if (const CPCodec* codec = cpcodec_find_decoder_by_name(config.codec_name)) {
return codec;
}
}
if (config.codec_id != CPCodecID::Unknow) {
if (const CPCodec* codec = cpcodec_find_decoder(config.codec_id)) {
return codec;
}
}
return nullptr;
}
} // namespace
struct CPStreamCore::Impl {
struct Packet {
std::vector<std::uint8_t> payload;
std::int64_t pts = 0;
bool end_of_stream = false;
bool flush = false;
};
explicit Impl(CPStreamConfig config)
: config_(std::move(config)) {
normalize_config();
}
~Impl() = default;
void normalize_config() {
if (config_.read_chunk_size == 0U) {
config_.read_chunk_size = 256U;
}
if (config_.packet_queue_capacity == 0U) {
config_.packet_queue_capacity = 1U;
}
if (config_.frame_queue_capacity == 0U) {
config_.frame_queue_capacity = 1U;
}
frame_queue_capacity_ = config_.frame_queue_capacity;
}
bool open(const CPStreamConfig& cfg) {
stop();
close();
config_ = cfg;
normalize_config();
if (config_.port.empty()) {
set_last_error("serial port is empty");
return false;
}
codec_descriptor_ = resolve_requested_codec(config_);
if (!codec_descriptor_) {
set_last_error("codec not found for requested identifier");
return false;
}
codec_ctx_ = cpcodec_alloc_context(codec_descriptor_);
if (!codec_ctx_) {
set_last_error("failed to allocate codec context");
return false;
}
int rc = cpcodec_open(codec_ctx_, codec_descriptor_);
if (rc < CP_SUCCESS) {
set_last_error("failed to open codec context: error " + std::to_string(rc));
cpcodec_free_context(&codec_ctx_);
codec_ctx_ = nullptr;
return false;
}
try {
auto serial = std::make_shared<serial::Serial>(
config_.port,
config_.baudrate,
config_.timeout,
config_.bytesize,
config_.parity,
config_.stopbits,
config_.flowcontrol);
serial->open();
serial->flush();
{
std::lock_guard lock(serial_mutex_);
serial_ = std::move(serial);
}
} catch (const serial::IOException& ex) {
set_last_error(ex.what() ? ex.what() : "serial IO exception");
cpcodec_close(codec_ctx_);
cpcodec_free_context(&codec_ctx_);
codec_ctx_ = nullptr;
return false;
} catch (const serial::SerialException& ex) {
set_last_error(ex.what() ? ex.what() : "serial exception");
cpcodec_close(codec_ctx_);
cpcodec_free_context(&codec_ctx_);
codec_ctx_ = nullptr;
return false;
} catch (const std::exception& ex) {
set_last_error(ex.what());
cpcodec_close(codec_ctx_);
cpcodec_free_context(&codec_ctx_);
codec_ctx_ = nullptr;
return false;
}
{
std::lock_guard lock(packet_mutex_);
packet_queue_.clear();
}
{
std::lock_guard lock(frame_mutex_);
frame_queue_.clear();
}
pts_counter_.store(0, std::memory_order_relaxed);
stop_requested_.store(false, std::memory_order_release);
set_last_error({});
return true;
}
bool open() {
return open(config_);
}
bool reopen(const CPStreamConfig& cfg) {
return open(cfg);
}
void close() {
stop();
{
std::lock_guard lock(serial_mutex_);
if (serial_) {
try {
if (serial_->isOpen()) {
serial_->close();
}
} catch (...) {
// Ignore close errors.
}
serial_.reset();
}
}
if (codec_ctx_) {
cpcodec_close(codec_ctx_);
cpcodec_free_context(&codec_ctx_);
codec_ctx_ = nullptr;
}
{
std::lock_guard lock(packet_mutex_);
packet_queue_.clear();
}
{
std::lock_guard lock(frame_mutex_);
frame_queue_.clear();
}
}
bool start() {
if (running_.load(std::memory_order_acquire)) {
return true;
}
std::shared_ptr<serial::Serial> serial_copy;
{
std::lock_guard lock(serial_mutex_);
serial_copy = serial_;
}
if (!serial_copy || !serial_copy->isOpen()) {
set_last_error("serial port is not open");
return false;
}
if (!codec_ctx_ || !codec_ctx_->is_open) {
set_last_error("codec context is not ready");
return false;
}
stop_requested_.store(false, std::memory_order_release);
running_.store(true, std::memory_order_release);
reader_thread_ = std::thread(&Impl::reader_loop, this);
decoder_thread_ = std::thread(&Impl::decoder_loop, this);
return true;
}
void stop() {
if (!running_.exchange(false, std::memory_order_acq_rel)) {
return;
}
stop_requested_.store(true, std::memory_order_release);
packet_cv_.notify_all();
if (reader_thread_.joinable()) {
reader_thread_.join();
}
signal_decoder_flush(true);
packet_cv_.notify_all();
if (decoder_thread_.joinable()) {
decoder_thread_.join();
}
stop_requested_.store(false, std::memory_order_release);
{
std::lock_guard lock(packet_mutex_);
packet_queue_.clear();
}
if (codec_ctx_ && codec_ctx_->is_open) {
reset_decoder();
}
}
bool is_open() const {
std::lock_guard lock(serial_mutex_);
return serial_ && serial_->isOpen();
}
bool is_running() const {
return running_.load(std::memory_order_acquire);
}
bool send(const std::vector<std::uint8_t>& data) {
return send(data.data(), data.size());
}
bool send(const std::uint8_t* data, std::size_t size) {
if (!data || size == 0U) {
return false;
}
std::shared_ptr<serial::Serial> serial_copy;
{
std::lock_guard lock(serial_mutex_);
serial_copy = serial_;
}
if (!serial_copy || !serial_copy->isOpen()) {
set_last_error("serial port is not open");
return false;
}
try {
const auto written = serial_copy->write(data, size);
return written == size;
} catch (const serial::IOException& ex) {
set_last_error(ex.what() ? ex.what() : "serial IO exception");
} catch (const serial::SerialException& ex) {
set_last_error(ex.what() ? ex.what() : "serial exception");
} catch (const std::exception& ex) {
set_last_error(ex.what());
}
return false;
}
std::optional<DecodedFrame> try_pop_frame() {
std::lock_guard lock(frame_mutex_);
if (frame_queue_.empty()) {
return std::nullopt;
}
DecodedFrame frame = std::move(frame_queue_.front());
frame_queue_.pop_front();
return frame;
}
bool wait_for_frame(DecodedFrame& frame, std::chrono::milliseconds timeout) {
std::unique_lock lock(frame_mutex_);
if (!frame_cv_.wait_for(lock, timeout, [&] {
return !frame_queue_.empty();
})) {
return false;
}
frame = std::move(frame_queue_.front());
frame_queue_.pop_front();
return true;
}
void clear_frames() {
std::lock_guard lock(frame_mutex_);
frame_queue_.clear();
}
void set_frame_queue_capacity(std::size_t capacity) {
if (capacity == 0U) {
capacity = 1U;
}
{
std::lock_guard lock(frame_mutex_);
frame_queue_capacity_ = capacity;
config_.frame_queue_capacity = capacity;
while (frame_queue_.size() > frame_queue_capacity_) {
frame_queue_.pop_front();
}
}
}
void set_frame_callback(FrameCallback callback) {
std::lock_guard lock(callback_mutex_);
frame_callback_ = std::move(callback);
}
CPStreamConfig config() const {
return config_;
}
std::string last_error() const {
std::lock_guard lock(last_error_mutex_);
return last_error_;
}
static std::vector<serial::PortInfo> list_ports() {
return serial::list_ports();
}
void reader_loop() {
std::vector<std::uint8_t> buffer(config_.read_chunk_size);
while (!stop_requested_.load(std::memory_order_acquire)) {
std::shared_ptr<serial::Serial> serial_copy;
{
std::lock_guard lock(serial_mutex_);
serial_copy = serial_;
}
if (!serial_copy || !serial_copy->isOpen()) {
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
}
std::size_t bytes_read = 0;
try {
bytes_read = serial_copy->read(buffer.data(), buffer.size());
} catch (const serial::IOException& ex) {
set_last_error(ex.what() ? ex.what() : "serial IO exception");
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
} catch (const serial::SerialException& ex) {
set_last_error(ex.what() ? ex.what() : "serial exception");
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
} catch (const std::exception& ex) {
set_last_error(ex.what());
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
}
if (bytes_read == 0U) {
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
}
Packet packet;
packet.payload.assign(buffer.begin(), buffer.begin() + static_cast<std::ptrdiff_t>(bytes_read));
packet.pts = pts_counter_.fetch_add(1, std::memory_order_relaxed);
{
std::lock_guard lock(packet_mutex_);
if (packet_queue_.size() >= config_.packet_queue_capacity) {
packet_queue_.pop_front();
}
packet_queue_.push_back(std::move(packet));
}
packet_cv_.notify_one();
}
}
void decoder_loop() {
while (true) {
Packet packet;
{
std::unique_lock lock(packet_mutex_);
packet_cv_.wait(lock, [&] {
return stop_requested_.load(std::memory_order_acquire) || !packet_queue_.empty();
});
if (packet_queue_.empty()) {
if (stop_requested_.load(std::memory_order_acquire)) {
break;
}
continue;
}
packet = std::move(packet_queue_.front());
packet_queue_.pop_front();
}
if (!codec_ctx_ || !codec_ctx_->is_open) {
if (packet.end_of_stream) {
break;
}
std::this_thread::sleep_for(kDecoderIdleSleep);
continue;
}
CPPacket cp_packet;
cp_packet.payload = std::move(packet.payload);
cp_packet.pts = packet.pts;
cp_packet.dts = packet.pts;
cp_packet.end_of_stream = packet.end_of_stream;
cp_packet.flush = packet.flush;
int rc = cpcodec_send_packet(codec_ctx_, &cp_packet);
if (rc < CP_SUCCESS) {
if (packet.end_of_stream) {
break;
}
continue;
}
while (true) {
CPFrame frame;
rc = cpcodec_receive_frame(codec_ctx_, &frame);
if (rc == CP_SUCCESS) {
DecodedFrame decoded;
decoded.pts = frame.pts;
decoded.received_at = std::chrono::steady_clock::now();
decoded.frame = std::move(frame);
FrameCallback callback_copy;
{
std::lock_guard lock(callback_mutex_);
callback_copy = frame_callback_;
}
if (callback_copy) {
callback_copy(decoded);
}
{
std::lock_guard lock(frame_mutex_);
if (frame_queue_.size() >= frame_queue_capacity_) {
frame_queue_.pop_front();
}
frame_queue_.push_back(std::move(decoded));
}
frame_cv_.notify_one();
} else if (rc == CP_ERROR_EAGAIN) {
break;
} else {
if (rc == CP_ERROR_EOF && packet.end_of_stream) {
return;
}
break;
}
}
if (packet.end_of_stream) {
break;
}
}
}
void signal_decoder_flush(bool end_of_stream) {
Packet packet;
packet.flush = true;
packet.end_of_stream = end_of_stream;
{
std::lock_guard lock(packet_mutex_);
packet_queue_.push_back(std::move(packet));
}
packet_cv_.notify_one();
}
void reset_decoder() {
if (!codec_ctx_ || !codec_descriptor_) {
return;
}
cpcodec_close(codec_ctx_);
int rc = cpcodec_open(codec_ctx_, codec_descriptor_);
if (rc < CP_SUCCESS) {
set_last_error("failed to reset codec context: error " + std::to_string(rc));
}
}
void set_last_error(std::string message) {
std::lock_guard lock(last_error_mutex_);
last_error_ = std::move(message);
}
CPStreamConfig config_{};
const CPCodec* codec_descriptor_ = nullptr;
std::shared_ptr<serial::Serial> serial_;
mutable std::mutex serial_mutex_;
CPCodecContext* codec_ctx_ = nullptr;
std::thread reader_thread_;
std::thread decoder_thread_;
std::mutex packet_mutex_;
std::condition_variable packet_cv_;
std::deque<Packet> packet_queue_;
std::mutex frame_mutex_;
std::condition_variable frame_cv_;
std::deque<DecodedFrame> frame_queue_;
std::size_t frame_queue_capacity_ = 16;
FrameCallback frame_callback_;
mutable std::mutex callback_mutex_;
std::atomic<bool> running_{false};
std::atomic<bool> stop_requested_{false};
std::atomic<std::int64_t> pts_counter_{0};
std::string last_error_;
mutable std::mutex last_error_mutex_;
};
CPStreamCore::CPStreamCore(CPStreamConfig config)
: impl_(std::make_unique<Impl>(std::move(config))) {}
CPStreamCore::~CPStreamCore() {
if (impl_) {
impl_->stop();
impl_->close();
}
}
bool CPStreamCore::open(const CPStreamConfig& config) {
return impl_->open(config);
}
bool CPStreamCore::open() {
return impl_->open();
}
bool CPStreamCore::reopen(const CPStreamConfig& config) {
return impl_->reopen(config);
}
void CPStreamCore::close() {
impl_->close();
}
bool CPStreamCore::start() {
return impl_->start();
}
void CPStreamCore::stop() {
impl_->stop();
}
bool CPStreamCore::is_open() const noexcept {
return impl_->is_open();
}
bool CPStreamCore::is_running() const noexcept {
return impl_->is_running();
}
bool CPStreamCore::send(const std::vector<std::uint8_t>& data) {
return impl_->send(data);
}
bool CPStreamCore::send(const std::uint8_t* data, std::size_t size) {
return impl_->send(data, size);
}
std::optional<DecodedFrame> CPStreamCore::try_pop_frame() {
return impl_->try_pop_frame();
}
bool CPStreamCore::wait_for_frame(DecodedFrame& frame, std::chrono::milliseconds timeout) {
return impl_->wait_for_frame(frame, timeout);
}
void CPStreamCore::clear_frames() {
impl_->clear_frames();
}
void CPStreamCore::set_frame_queue_capacity(std::size_t capacity) {
impl_->set_frame_queue_capacity(capacity);
}
void CPStreamCore::set_frame_callback(FrameCallback callback) {
impl_->set_frame_callback(std::move(callback));
}
CPStreamConfig CPStreamCore::config() const {
return impl_->config();
}
std::string CPStreamCore::last_error() const {
return impl_->last_error();
}
std::vector<serial::PortInfo> CPStreamCore::list_available_ports() {
return Impl::list_ports();
}
} // namespace ffmsep

77
components/ffmsep/cpstream_core.hh Normal file
View File

@@ -0,0 +1,77 @@
#pragma once
#include "components/ffmsep/cpdecoder.hh"
#include <chrono>
#include <cstdint>
#include <functional>
#include <memory>
#include <optional>
#include <serial/serial.h>
#include <string>
#include <vector>
namespace ffmsep {
struct DecodedFrame {
CPFrame frame;
std::chrono::steady_clock::time_point received_at{};
std::int64_t pts = 0;
};
struct CPStreamConfig {
std::string port;
std::uint32_t baudrate = 115200;
serial::Timeout timeout = serial::Timeout::simpleTimeout(50);
serial::bytesize_t bytesize = serial::eightbits;
serial::parity_t parity = serial::parity_none;
serial::stopbits_t stopbits = serial::stopbits_one;
serial::flowcontrol_t flowcontrol = serial::flowcontrol_none;
std::size_t read_chunk_size = 256;
std::size_t packet_queue_capacity = 128;
std::size_t frame_queue_capacity = 16;
CPCodecID codec_id = CPCodecID::Unknow;
std::string codec_name;
};
class CPStreamCore {
public:
using FrameCallback = std::function<void(const DecodedFrame&)>;
explicit CPStreamCore(CPStreamConfig config = {});
~CPStreamCore();
CPStreamCore(const CPStreamCore&) = delete;
CPStreamCore& operator=(const CPStreamCore&) = delete;
bool open(const CPStreamConfig& config);
bool open();
bool reopen(const CPStreamConfig& config);
void close();
bool start();
void stop();
[[nodiscard]] bool is_open() const noexcept;
[[nodiscard]] bool is_running() const noexcept;
bool send(const std::vector<std::uint8_t>& data);
bool send(const std::uint8_t* data, std::size_t size);
std::optional<DecodedFrame> try_pop_frame();
bool wait_for_frame(DecodedFrame& frame, std::chrono::milliseconds timeout);
void clear_frames();
void set_frame_queue_capacity(std::size_t capacity);
void set_frame_callback(FrameCallback callback);
[[nodiscard]] CPStreamConfig config() const;
[[nodiscard]] std::string last_error() const;
static std::vector<serial::PortInfo> list_available_ports();
private:
struct Impl;
std::unique_ptr<Impl> impl_;
};
} // namespace ffmsep

95
components/ffmsep/tactile/tecdec.cc → components/ffmsep/tactile/tacdec.cc
View File

@@ -1,28 +1,25 @@
//
// Decoder for the tactile sensor framed binary protocol.
//
#include "tacdec.h"
#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <vector>
namespace ffmsep::tactile {
namespace {
constexpr std::size_t kMinimumFrameSize = 1 // start
+ 1 // address
+ 1 // function
+ 1 // length
+ 0 // payload
+ 2 // CRC
+ 2; // end markers
constexpr std::size_t kMinimumFrameSize = 1
+ 1
+ 1
+ 1
+ 0
+ 2
+ 2;
constexpr std::uint16_t kCrcInitial = 0xFFFF;
constexpr std::uint16_t kCrcPolynomial = 0xA001; // CRC-16/MODBUS (LSB first)
constexpr std::uint16_t kCrcPolynomial = 0xA001;
struct TactileDecoderContext {
std::vector<std::uint8_t> fifo;
@@ -30,6 +27,14 @@ struct TactileDecoderContext {
std::int64_t next_pts = 0;
};
std::size_t frame_length_from_payload(std::uint8_t payload_length) {
return 1U + 1U + 1U + 1U + payload_length + 2U + 2U;
}
const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
return buf.empty() ? nullptr : buf.data();
}
std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) {
std::uint16_t crc = kCrcInitial;
for (std::size_t i = 0; i < length; ++i) {
@@ -37,7 +42,8 @@ std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) {
for (int bit = 0; bit < 8; ++bit) {
if ((crc & 0x0001U) != 0U) {
crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial);
} else {
}
else {
crc = static_cast<std::uint16_t>(crc >> 1U);
}
}
@@ -71,11 +77,10 @@ void tactile_close(CPCodecContext* ctx) {
}
int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) {
auto* priv = get_priv(ctx);
auto priv = get_priv(ctx);
if (!priv) {
return CP_ERROR_INVALID_STATE;
}
if (packet.flush) {
priv->fifo.clear();
priv->end_of_stream = false;
@@ -93,14 +98,6 @@ int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) {
return CP_SUCCESS;
}
std::size_t frame_length_from_payload(std::uint8_t payload_length) {
return 1U + 1U + 1U + 1U + payload_length + 2U + 2U;
}
const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
return buf.empty() ? nullptr : buf.data();
}
int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
auto* priv = get_priv(ctx);
if (!priv) {
@@ -118,7 +115,6 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
return CP_ERROR_EAGAIN;
}
// Discard bytes until start byte is found.
auto start_it = std::find(buf.begin(), buf.end(), kStartByte);
if (start_it == buf.end()) {
buf.clear();
@@ -128,13 +124,13 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
}
return CP_ERROR_EAGAIN;
}
if (start_it != buf.begin()) {
buf.erase(buf.begin(), start_it);
}
if (buf.size() < kMinimumFrameSize) {
if (priv->end_of_stream) {
// Incomplete frame at end of stream: drop it and report EOF.
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
@@ -143,14 +139,13 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
}
const std::uint8_t* data = buffer_data(buf);
const std::uint8_t address = data[1];
const std::uint8_t function = data[2];
const std::uint8_t payload_length = data[3];
const std::uint8_t address = data[1U];
const FunctionCode function = static_cast<FunctionCode>(data[2U]);
const std::uint8_t payload_length = data[3U];
const std::size_t total_frame_length = frame_length_from_payload(payload_length);
if (buf.size() < total_frame_length) {
if (priv->end_of_stream) {
// Not enough data before stream end: treat as EOF and drop buffer.
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
@@ -171,12 +166,11 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
const std::uint8_t end_second = data[end_offset + 1U];
if (end_first != kEndByteFirst || end_second != kEndByteSecond) {
// Invalid end marker, drop start byte and retry.
buf.erase(buf.begin());
continue;
}
const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload
const std::size_t crc_region_length = 3U + payload_length;
const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length);
if (computed_crc != crc_value) {
buf.erase(buf.begin());
@@ -192,24 +186,22 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
frame.valid = true;
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
return CP_SUCCESS;
}
}
const CPCodec kTactileCodec {
"tactile_serial",
"Framed tactile sensor serial protocol decoder",
CPMediaType::Data,
CPCodecID::Tactile,
sizeof(TactileDecoderContext),
&tactile_init,
&tactile_close,
&tactile_send_packet,
&tactile_receive_frame
.name = "tactile_serial",
.long_name = "Framed tactile sensor serial protocol decoder",
.type = CPMediaType::Data,
.id = CPCodecID::Tactile,
.priv_data_size = sizeof(TactileDecoderContext),
.init = &tactile_init,
.close = &tactile_close,
.send_packet = &tactile_send_packet,
.receive_frame = &tactile_receive_frame
};
} // namespace
}
std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
@@ -221,6 +213,7 @@ std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
if (bytes[0] != kStartByte) {
return std::nullopt;
}
if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) {
return std::nullopt;
}
@@ -233,7 +226,6 @@ std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
if (frame_length_from_payload(length) != size) {
return std::nullopt;
}
const std::uint8_t address = bytes[1];
const FunctionCode function = static_cast<FunctionCode>(bytes[2]);
const std::size_t payload_offset = 4U;
@@ -271,10 +263,6 @@ std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame) {
return size;
}
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame) {
return parse_matrix_size_payload(frame);
}
const CPCodec* tactile_codec() {
return &kTactileCodec;
}
@@ -282,5 +270,4 @@ const CPCodec* tactile_codec() {
void register_tactile_codec() {
cpcodec_register(&kTactileCodec);
}
} // namespace ffmsep::tactile
}

33
components/ffmsep/tactile/tacdec.h → components/ffmsep/tactile/tacdec.hh
View File

@@ -1,29 +1,23 @@
//
// High level helpers for the tactile sensor binary protocol.
//
#pragma once
#include "../cpdecoder.hh"
#include "cpdecoder.hh"
#include <cstdint>
#include <optional>
#include <vector>
namespace ffmsep::tactile {
inline constexpr std::uint8_t kStartByte = 0x3A;
inline constexpr std::uint8_t kEndByteFirst = 0x0D;
inline constexpr std::uint8_t kEndByteSecond = 0x0A;
enum class FunctionCode : std::uint8_t {
Unknown = 0x00,
ReadMatrix = 0x01,
ReadSingle = 0x02,
ReadTemperature = 0x03,
SetDeviceId = 0x51,
SetMatrixSize = 0x52,
CalibrationMode = 0x53
Unknown = 0x00,
ReadMatrix = 0x01,
ReadSingle = 0x02,
ReadTemperature = 0x03,
SetDeviceId = 0x51,
SetMatrixSize = 0x52,
CalibrationMode = 0x53,
};
struct MatrixSize {
@@ -32,18 +26,17 @@ struct MatrixSize {
};
struct TactileFrame {
std::uint8_t device_address = 0;
FunctionCode function = FunctionCode::Unknown;
std::uint8_t data_length = 0;
std::uint8_t device_address = 0;
FunctionCode function = FunctionCode::Unknown;
std::uint8_t data_length = 0;
std::vector<std::uint8_t> payload;
};
std::optional<TactileFrame> parse_frame(const CPFrame& frame);
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame);
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame);
std::optional<MatrixSize> parse_patrix_coordinate_payload(const TactileFrame& frame);
const CPCodec* tactile_codec();
void register_tactile_codec();
} // namespace ffmsep::tactile
}

15
components/view.cc
View File

@@ -70,24 +70,17 @@ static auto ComConfigComponent(ThemeManager& manager, auto&& callback) {
slpro::LeadingIcon { material::icon::kArrowDropDown, material::regular::font},
slpro::IndexChanged {[&](auto& self){ qDebug() << self.currentIndex();}},
slpro::LeadingText {"Baud"},
// slpro::MutableItems {select_baud_context},
MutableForward {
slpro::SelectItems {},
select_baud_context,
}
},
// lnpro::Item<MatSelect> {
// // slpro::ThemeManager {manager},
// // slpro::LeadingIcon {material::icon::kArrowDropDown, material::regular::font},
// // slpro::IndexChanged {}
// // }
lnpro::SpacingItem {20},
lnpro::Item<IconButton> {
ibpro::ThemeManager {manager},
ibpro::FixedSize {40, 40},
ibpro::Color { IconButton::Color::TONAL },
ibpro::Font { material::kRoundSmallFont },
// ibpro::FontIcon { material::icon::kFavorite },
ibpro::FontIcon { material::icon::kAddLink },
ibpro::Clickable {[slogen_context] {
constexpr auto random_slogen = [] {
@@ -113,15 +106,14 @@ static auto ComConfigComponent(ThemeManager& manager, auto&& callback) {
ibpro::Font { material::kRoundSmallFont },
ibpro::FontIcon { material::icon::kRefresh },
ibpro::Clickable {[select_baud_context] {
// 定义两组不同的选项
static constexpr auto options_group1 = std::array {
"第一组选项1", "第一组选项2", "第一组选项3"
};
static constexpr auto options_group2 = std::array {
"第二组选项A", "第二组选项B", "第二组选项C", "第二组选项D"
};
// 随机选择一组选项
static std::random_device rd;
static std::mt19937 gen(rd());
std::uniform_int_distribution<> dist(0, 1);
@@ -136,9 +128,6 @@ static auto ComConfigComponent(ThemeManager& manager, auto&& callback) {
new_options << QString::fromUtf8(option);
}
}
qDebug() << new_options;
// 更新选项列表，MatSelect会自动刷新
*select_baud_context = new_options;
}},
},