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

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
}