298 lines
10 KiB
C++
298 lines
10 KiB
C++
//
|
||
// 触觉传感器串口协议解码器实现（中文注释版）。
|
||
// 使用要点：
|
||
// 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
|