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

颜色修改红绿，修复帧错误卡顿bug

Browse Source
This commit is contained in:
lenn
2025-12-16 14:25:48 +08:00
parent c86c24488c
commit a1f7f337c2
15 changed files with 2201 additions and 917 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
components/ffmsep/cpdecoder.hh
View File

@@ -21,10 +21,11 @@ enum class CPMediaType : std::uint8_t {
Data,
};
enum class CPCodecID : std::uint32_t {
Unknow = 0,
Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识 Tactile Quick Codec Type
};
enum class CPCodecID : std::uint32_t {
Unknow = 0,
Tactile = 0x54514354u, // 'T','Q','C','T'：触觉传感器协议标识 Tactile Quick Codec Type
PiezoresistiveB = 0x54514342u // 'T','Q','C','B'：压阻B测试协议
};
struct CPPacket {
std::vector<std::uint8_t> payload;
@@ -121,4 +122,4 @@ int cpcodec_close(CPCodecContext*);
void cpcodec_free_context(CPCodecContext **ctx);
int cpcodec_send_packet(CPCodecContext*, const CPPacket*);
int cpcodec_receive_frame(CPCodecContext*, CPFrame*);
}
}

154
components/ffmsep/cpstream_core.cc
View File

@@ -2,7 +2,7 @@
#include "components/ffmsep/presist/presist.hh"
#include "dlog/dlog.hh"
#include <iostream>
#include <algorithm>
#include <atomic>
#include <chrono>
@@ -11,12 +11,16 @@
#include <cstdint>
#include <deque>
#include <future>
#include <ios>
#include <memory>
#include <mutex>
#include <optional>
#include <qlogging.h>
#include <thread>
#include <utility>
#include <vector>
#include <qdebug.h>
#include <iostream>
using namespace std::chrono_literals;
@@ -24,7 +28,7 @@ namespace ffmsep {
namespace {
constexpr auto kReaderIdleSleep = 5ms;
constexpr auto kReaderIdleSleep = 5ms;
constexpr auto kDecoderIdleSleep = 1ms;
const CPCodec* resolve_requested_codec(const CPStreamConfig& config) {
@@ -46,13 +50,12 @@ const CPCodec* resolve_requested_codec(const CPStreamConfig& config) {
struct CPStreamCore::Impl {
struct Packet {
std::vector<std::uint8_t> payload;
std::int64_t pts = 0;
bool end_of_stream = false;
bool flush = false;
std::int64_t pts = 0;
bool end_of_stream = false;
bool flush = false;
};
explicit Impl(CPStreamConfig config)
: config_(std::move(config)) {
explicit Impl(CPStreamConfig config): config_(std::move(config)) {
normalize_config();
frame_writer_ = std::make_unique<persist::JsonWritter>();
}
@@ -109,13 +112,13 @@ struct CPStreamCore::Impl {
try {
auto serial = std::make_shared<serial::Serial>(
config_.port,
config_.baudrate,
config_.timeout,
config_.bytesize,
config_.parity,
config_.stopbits,
config_.flowcontrol);
config_.port,
config_.baudrate,
config_.timeout,
config_.bytesize,
config_.parity,
config_.stopbits,
config_.flowcontrol);
if (!serial->isOpen()) {
serial->open();
}
@@ -125,19 +128,22 @@ struct CPStreamCore::Impl {
std::lock_guard<std::mutex> lock(serial_mutex_);
serial_ = std::move(serial);
}
} catch (const serial::IOException& ex) {
}
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) {
}
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) {
}
catch (const std::exception& ex) {
set_last_error(ex.what());
cpcodec_close(codec_ctx_);
cpcodec_free_context(&codec_ctx_);
@@ -177,7 +183,8 @@ struct CPStreamCore::Impl {
if (serial_->isOpen()) {
serial_->close();
}
} catch (...) {
}
catch (...) {
// Ignore close errors.
}
serial_.reset();
@@ -224,7 +231,7 @@ struct CPStreamCore::Impl {
stop_requested_.store(false, std::memory_order_release);
running_.store(true, std::memory_order_release);
reader_thread_ = std::thread(&Impl::reader_loop, this);
reader_thread_ = std::thread(&Impl::reader_loop, this);
decoder_thread_ = std::thread(&Impl::decoder_loop, this);
if (!config_.slave_request_command.empty()) {
slave_thread_ = std::thread(&Impl::slave_loop, this);
@@ -298,11 +305,14 @@ struct CPStreamCore::Impl {
try {
const auto written = serial_copy->write(data, size);
return written == size;
} catch (const serial::IOException& ex) {
}
catch (const serial::IOException& ex) {
set_last_error(ex.what() ? ex.what() : "serial IO exception");
} catch (const serial::SerialException& ex) {
}
catch (const serial::SerialException& ex) {
set_last_error(ex.what() ? ex.what() : "serial exception");
} catch (const std::exception& ex) {
}
catch (const std::exception& ex) {
set_last_error(ex.what());
}
return false;
@@ -341,7 +351,7 @@ struct CPStreamCore::Impl {
}
{
std::lock_guard<std::mutex> lock(frame_mutex_);
frame_queue_capacity_ = capacity;
frame_queue_capacity_ = capacity;
config_.frame_queue_capacity = capacity;
while (frame_queue_.size() > frame_queue_capacity_) {
frame_queue_.pop_front();
@@ -375,12 +385,11 @@ struct CPStreamCore::Impl {
if (snapshot.empty()) {
std::promise<persist::WriteResult> promise;
auto future = promise.get_future();
auto future = promise.get_future();
promise.set_value(persist::WriteResult{
false,
"no recorded frames available",
path
});
false,
"no recorded frames available",
path });
return future;
}
@@ -409,6 +418,7 @@ struct CPStreamCore::Impl {
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<std::mutex> lock(serial_mutex_);
@@ -422,15 +432,19 @@ struct CPStreamCore::Impl {
std::size_t bytes_read = 0;
try {
bytes_read = serial_copy->read(buffer.data(), buffer.size());
} catch (const serial::IOException& ex) {
qDebug() << "bytes_read: " << bytes_read;
}
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) {
}
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) {
}
catch (const std::exception& ex) {
set_last_error(ex.what());
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
@@ -440,9 +454,26 @@ struct CPStreamCore::Impl {
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
}
const auto format_command =
[](const std::vector<std::uint8_t>& data) -> std::string {
if (data.empty()) {
return "[]";
}
std::ostringstream oss;
oss << '[' << std::uppercase << std::setfill('0');
for (std::size_t idx = 0; idx < data.size(); ++idx) {
if (idx != 0U) {
oss << ' ';
}
oss << std::setw(2) << std::hex << static_cast<unsigned int>(data[idx]);
}
oss << ']';
return oss.str();
};
Packet packet;
packet.payload.assign(buffer.begin(), buffer.begin() + static_cast<std::ptrdiff_t>(bytes_read));
// std::cout << "======payload======" << std::endl;
// std::cout << format_command(packet.payload) << std::endl;
packet.pts = pts_counter_.fetch_add(1, std::memory_order_relaxed);
{
@@ -457,8 +488,8 @@ struct CPStreamCore::Impl {
}
void slave_loop() {
const auto command = config_.slave_request_command;
auto interval = config_.slave_request_interval;
const auto command = config_.slave_request_command;
auto interval = config_.slave_request_interval;
if (interval.count() < 0) {
interval = 0ms;
}
@@ -512,11 +543,11 @@ struct CPStreamCore::Impl {
}
CPPacket cp_packet;
cp_packet.payload = std::move(packet.payload);
cp_packet.pts = packet.pts;
cp_packet.dts = packet.pts;
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;
cp_packet.flush = packet.flush;
int rc = cpcodec_send_packet(codec_ctx_, &cp_packet);
if (rc < CP_SUCCESS) {
@@ -530,20 +561,24 @@ struct CPStreamCore::Impl {
CPFrame frame;
rc = cpcodec_receive_frame(codec_ctx_, &frame);
if (rc == CP_SUCCESS) {
auto decoded = std::make_shared<DecodedFrame>();
decoded->pts = frame.pts;
auto decoded = std::make_shared<DecodedFrame>();
decoded->pts = frame.pts;
decoded->received_at = std::chrono::steady_clock::now();
decoded->frame = std::move(frame);
decoded->id = codec_descriptor_ ? codec_descriptor_->id : CPCodecID::Unknow;
decoded->frame = std::move(frame);
decoded->id = codec_descriptor_ ? codec_descriptor_->id : CPCodecID::Unknow;
if (decoded->id == CPCodecID::Tactile) {
if (auto parsed = tactile::parse_frame(decoded->frame)) {
decoded->tactile = parsed;
decoded->tactile = parsed;
decoded->tactile_pressures = tactile::parse_pressure_values(*parsed);
if (auto matrix = tactile::parse_matrix_size_payload(*parsed)) {
decoded->tactile_matrix_size = matrix;
}
}
}
else if (decoded->id == CPCodecID::PiezoresistiveB) {
decoded->tactile_pressures =
tactile::parse_piezoresistive_b_pressures(decoded->frame);
}
FrameCallback callback_copy;
{
@@ -560,14 +595,16 @@ struct CPStreamCore::Impl {
frame_queue_.pop_front();
}
frame_queue_.push_back(decoded);
if (decoded->id == CPCodecID::Tactile) {
if (decoded->id == CPCodecID::Tactile || decoded->id == CPCodecID::PiezoresistiveB) {
frame_record_queue_.push_back(decoded);
}
}
frame_cv_.notify_one();
} else if (rc == CP_ERROR_EAGAIN) {
}
else if (rc == CP_ERROR_EAGAIN) {
break;
} else {
}
else {
if (rc == CP_ERROR_EOF && packet.end_of_stream) {
return;
}
@@ -583,7 +620,7 @@ struct CPStreamCore::Impl {
void signal_decoder_flush(bool end_of_stream) {
Packet packet;
packet.flush = true;
packet.flush = true;
packet.end_of_stream = end_of_stream;
{
std::lock_guard<std::mutex> lock(packet_mutex_);
@@ -612,7 +649,7 @@ struct CPStreamCore::Impl {
const CPCodec* codec_descriptor_ = nullptr;
std::shared_ptr<serial::Serial> serial_;
mutable std::mutex serial_mutex_;
mutable std::mutex serial_mutex_;
CPCodecContext* codec_ctx_ = nullptr;
@@ -620,33 +657,32 @@ struct CPStreamCore::Impl {
std::thread slave_thread_;
std::thread decoder_thread_;
std::mutex packet_mutex_;
std::mutex packet_mutex_;
std::condition_variable packet_cv_;
std::deque<Packet> packet_queue_;
std::deque<Packet> packet_queue_;
mutable std::mutex frame_mutex_;
mutable std::mutex frame_mutex_;
std::condition_variable frame_cv_;
// std::deque<DecodedFrame> frame_queue_;
// 更新为智能指针，我们需要更长的生命周期😊
std::deque<std::shared_ptr<DecodedFrame>> frame_queue_;
std::deque<std::shared_ptr<DecodedFrame>> frame_record_queue_;
std::size_t frame_queue_capacity_ = 16;
std::size_t frame_queue_capacity_ = 16;
FrameCallback frame_callback_;
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::atomic<bool> running_{ false };
std::atomic<bool> stop_requested_{ false };
std::atomic<std::int64_t> pts_counter_{ 0 };
std::string last_error_;
std::string last_error_;
mutable std::mutex last_error_mutex_;
std::unique_ptr<persist::JsonWritter> frame_writer_;
};
CPStreamCore::CPStreamCore(CPStreamConfig config)
: impl_(std::make_unique<Impl>(std::move(config))) {}
CPStreamCore::CPStreamCore(CPStreamConfig config): impl_(std::make_unique<Impl>(std::move(config))) {}
CPStreamCore::~CPStreamCore() {
if (impl_) {

156
components/ffmsep/presist/presist.cc
View File

@@ -11,6 +11,8 @@
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <nlohmann/json_fwd.hpp>
#include <sstream>
#include <system_error>
#include <nlohmann/json.hpp>
@@ -20,6 +22,33 @@ namespace {
using nlohmann::json;
// 旧的 JSON 导出实现保留在此，避免直接删除，便于回退。
// bool is_simple_array(const json& value) { ... }
// void dump_compact_json(...)
// json serialize_tactile_frame(const DecodedFrame& frame) { ... }
std::string payload_to_csv_row(const std::vector<std::uint8_t>& payload) {
// Combine every 2 bytes (little-endian) into one 16-bit value, output in decimal.
std::ostringstream oss;
bool first = true;
for (std::size_t idx = 0; idx + 1U < payload.size(); idx += 2U) {
const auto value =
static_cast<std::uint16_t>(payload[idx]) | static_cast<std::uint16_t>(payload[idx + 1U] << 8U);
if (!first) {
oss << ',';
}
first = false;
oss << value;
}
return oss.str();
}
} // namespace
namespace {
using nlohmann::json;
bool is_simple_array(const json& value) {
if (!value.is_array()) {
return false;
@@ -30,11 +59,11 @@ bool is_simple_array(const json& value) {
}
void dump_compact_json(std::ostream& out,
const json& value,
int indent = 0,
int indent_step = 2) {
const auto indent_str = std::string(static_cast<std::size_t>(indent), ' ');
const auto child_indent = indent + indent_step;
const json& value,
int indent = 0,
int indent_step = 2) {
const auto indent_str = std::string(static_cast<std::size_t>(indent), ' ');
const auto child_indent = indent + indent_step;
const auto child_indent_str = std::string(static_cast<std::size_t>(child_indent), ' ');
if (value.is_object()) {
@@ -48,7 +77,8 @@ void dump_compact_json(std::ostream& out,
out << child_indent_str << json(it.key()).dump() << ": ";
dump_compact_json(out, it.value(), child_indent, indent_step);
}
out << '\n' << indent_str << '}';
out << '\n'
<< indent_str << '}';
return;
}
@@ -72,7 +102,7 @@ void dump_compact_json(std::ostream& out,
out << "[\n";
bool first = true;
for (const auto& item : value) {
for (const auto& item: value) {
if (!first) {
out << ",\n";
}
@@ -80,7 +110,8 @@ void dump_compact_json(std::ostream& out,
out << child_indent_str;
dump_compact_json(out, item, child_indent, indent_step);
}
out << '\n' << indent_str << ']';
out << '\n'
<< indent_str << ']';
return;
}
@@ -89,38 +120,37 @@ void dump_compact_json(std::ostream& out,
json serialize_tactile_frame(const DecodedFrame& frame) {
json result = {
{"pts", frame.pts},
{"raw_frame", frame.frame.data},
{"pressures", frame.tactile_pressures},
{ "pts", frame.pts },
{ "raw_frame", frame.frame.data },
{ "pressures", frame.tactile_pressures },
};
const auto received = frame.received_at.time_since_epoch();
result["received_at_ns"] =
std::chrono::duration_cast<std::chrono::nanoseconds>(received).count();
std::chrono::duration_cast<std::chrono::nanoseconds>(received).count();
if (frame.tactile_matrix_size) {
result["matrix"] = {
{"long_edge", frame.tactile_matrix_size->long_edge},
{"short_edge", frame.tactile_matrix_size->short_edge},
{ "long_edge", frame.tactile_matrix_size->long_edge },
{ "short_edge", frame.tactile_matrix_size->short_edge },
};
}
if (frame.tactile) {
const auto& tactile = *frame.tactile;
result["tactile"] = {
{"device_address", tactile.device_address},
{"response_function", tactile.response_function},
{"function", static_cast<std::uint8_t>(tactile.function)},
{"start_address", tactile.start_address},
{"return_byte_count", tactile.return_byte_count},
{"status", tactile.status},
{"payload", tactile.payload},
result["tactile"] = {
{ "device_address", tactile.device_address },
{ "response_function", tactile.response_function },
{ "function", static_cast<std::uint8_t>(tactile.function) },
{ "start_address", tactile.start_address },
{ "return_byte_count", tactile.return_byte_count },
{ "status", tactile.status },
{ "payload", tactile.payload },
};
}
return result;
}
} // namespace
bool WriteQueue::push(WriteRequest&& req) {
@@ -158,21 +188,20 @@ void WriteQueue::stop() {
cond_.notify_all();
}
JsonWritter::JsonWritter()
: write_thread_([this] { run(); }) {}
JsonWritter::JsonWritter(): write_thread_([this] { run(); }) {}
JsonWritter::~JsonWritter() {
stop();
}
std::future<WriteResult> JsonWritter::enqueue(std::string path,
std::deque<std::shared_ptr<DecodedFrame>> frames) {
std::deque<std::shared_ptr<DecodedFrame>> frames) {
std::promise<WriteResult> promise;
auto future = promise.get_future();
auto future = promise.get_future();
WriteRequest request{std::move(path), std::move(frames), std::move(promise)};
WriteRequest request{ std::move(path), std::move(frames), std::move(promise) };
if (!write_queue_.push(std::move(request))) {
WriteResult result{false, "writer has been stopped", request.path};
WriteResult result{ false, "writer has been stopped", request.path };
request.promise.set_value(std::move(result));
}
@@ -185,62 +214,67 @@ void JsonWritter::run() {
try {
auto result = write_once(request.path, std::move(request.frames));
request.promise.set_value(std::move(result));
} catch (const std::exception& ex) {
request.promise.set_value(WriteResult{false, ex.what(), request.path});
} catch (...) {
request.promise.set_value(WriteResult{false, "unknown error", request.path});
}
catch (const std::exception& ex) {
request.promise.set_value(WriteResult{ false, ex.what(), request.path });
}
catch (...) {
request.promise.set_value(WriteResult{ false, "unknown error", request.path });
}
}
}
WriteResult JsonWritter::write_once(const std::string& path,
std::deque<std::shared_ptr<DecodedFrame>> frames) {
std::deque<std::shared_ptr<DecodedFrame>> frames) {
if (path.empty()) {
return {false, "export path is empty", path};
return { false, "export path is empty", path };
}
json tactile_frames = json::array();
for (const auto& frame : frames) {
if (!frame) {
continue;
}
if (frame->id != CPCodecID::Tactile || !frame->tactile) {
continue;
}
tactile_frames.push_back(serialize_tactile_frame(*frame));
}
if (tactile_frames.empty()) {
return {false, "no tactile frames available for export", path};
}
json root;
root["codec"] = "tactile";
root["frames"] = std::move(tactile_frames);
std::filesystem::path fs_path(path);
if (fs_path.has_parent_path()) {
std::error_code ec;
std::filesystem::create_directories(fs_path.parent_path(), ec);
if (ec) {
return {false, "failed to create export directory: " + ec.message(), path};
return { false, "failed to create export directory: " + ec.message(), path };
}
}
std::ofstream stream(path, std::ios::binary | std::ios::trunc);
if (!stream.is_open()) {
return {false, "failed to open export file", path};
return { false, "failed to open export file", path };
}
bool wrote_any = false;
for (const auto& frame: frames) {
if (!frame) {
continue;
}
std::vector<std::uint8_t> payload;
if (frame->id == CPCodecID::Tactile && frame->tactile) {
payload = frame->tactile->payload;
}
else if (frame->id == CPCodecID::PiezoresistiveB) {
payload = tactile::extract_piezoresistive_b_payload(frame->frame);
}
if (payload.empty()) {
continue;
}
const auto row = payload_to_csv_row(payload);
stream << row << '\n';
wrote_any = true;
}
dump_compact_json(stream, root);
stream << '\n';
stream.flush();
if (!stream.good()) {
return {false, "failed to write export file", path};
return { false, "failed to write export file", path };
}
if (!wrote_any) {
return { false, "no tactile frames available for export", path };
}
return {true, {}, path};
return { true, {}, path };
}
void JsonWritter::stop() {

540
components/ffmsep/tactile/tacdec.cc
View File

@@ -1,31 +1,61 @@
#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <array>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <vector>
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <qlogging.h>
#include <vector>
#include <qdebug.h>
#include <iostream>
namespace ffmsep::tactile {
namespace {
constexpr std::size_t kHeaderSize = 4U; // start bytes + length field
constexpr std::size_t kFixedSectionSize = 1U + 1U + 1U + 4U + 2U + 1U; // address..status
constexpr std::size_t kMinimumFrameSize = kHeaderSize + kFixedSectionSize + 1U; // + CRC byte
constexpr std::uint8_t kCrcPolynomial = 0x07U;
constexpr std::uint8_t kCrcInitial = 0x00U;
constexpr std::uint8_t kCrcXorOut = 0xA9U;
constexpr std::size_t kHeaderSize = 4U; // start bytes + length field
constexpr std::size_t kFixedSectionSize = 1U + 1U + 1U + 4U + 2U + 1U; // address..status
constexpr std::size_t kMinimumFrameSize = kHeaderSize + kFixedSectionSize + 1U; // + CRC byte
constexpr std::uint8_t kCrcPolynomial = 0x07U;
constexpr std::uint8_t kCrcInitial = 0x00U;
constexpr std::uint8_t kCrcXorOut = 0xA9U;
constexpr std::array<std::uint8_t, 2> kStartSequence{
kStartByteFirst,
kStartByteSecond
};
constexpr std::size_t kAbsoluteMaxPayloadBytes = 4096U; // 硬上限，防止异常配置撑爆内存
constexpr std::array<std::uint8_t, 2> kPiezoresistiveBStartSequence{
kPiezoresistiveBStartByteFirst,
kPiezoresistiveBStartByteSecond
};
constexpr std::array<std::uint8_t, 2> kPiezoresistiveBEndSequence{
kPiezoresistiveBEndByteFirst,
kPiezoresistiveBEndByteSecond
};
constexpr std::size_t kPiezoresistiveBPayloadSize =
kPiezoresistiveBValueCount * 2U;
constexpr std::size_t kPiezoresistiveBFrameSize =
kPiezoresistiveBStartSequence.size() + kPiezoresistiveBPayloadSize + kPiezoresistiveBEndSequence.size();
struct TactileDecoderContext {
std::vector<std::uint8_t> fifo;
bool end_of_stream = false;
std::int64_t next_pts = 0;
bool end_of_stream = false;
std::int64_t next_pts = 0;
CPCodecID codec_id = CPCodecID::Unknow;
std::size_t max_payload_bytes = kPiezoresistiveBPayloadSize;
std::size_t max_frame_bytes = kHeaderSize + kFixedSectionSize + kPiezoresistiveBPayloadSize + 1U;
std::size_t max_fifo_bytes = (kHeaderSize + kFixedSectionSize + kPiezoresistiveBPayloadSize + 1U) * 2U;
void update_limits(std::size_t payload_bytes) {
const auto clamped_payload = std::min<std::size_t>(
std::max<std::size_t>(payload_bytes, 2U),
kAbsoluteMaxPayloadBytes);
max_payload_bytes = clamped_payload;
max_frame_bytes = kHeaderSize + kFixedSectionSize + max_payload_bytes + 1U;
max_fifo_bytes = max_frame_bytes * 2U;
}
};
const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
@@ -33,67 +63,130 @@ const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
}
std::uint8_t crc8_with_xorout(const std::uint8_t* data, std::size_t length) {
#if 0
std::uint8_t reg = kCrcInitial;
for (std::size_t i = 0; i < length; ++i) {
reg ^= data[i];
for (int bit = 0; bit < 8; ++bit) {
if ((reg & 0x80U) != 0U) {
reg = static_cast<std::uint8_t>((reg << 1U) ^ kCrcPolynomial);
} else {
}
else {
reg = static_cast<std::uint8_t>(reg << 1U);
}
}
}
return static_cast<std::uint8_t>(reg ^ kCrcXorOut);
#endif
constexpr std::uint8_t kPolynomial = 0x07;
constexpr std::uint8_t kInitial = 0x00;
constexpr std::uint8_t kXorOut =
0x55; // CRC-8/ITU params match device expectation
std::uint8_t reg = kInitial;
for (std::size_t idx = 0; idx < length; ++idx) {
reg = static_cast<std::uint8_t>(reg ^ data[idx]);
for (int bit = 0; bit < 8; ++bit) {
if ((reg & 0x80U) != 0U) {
reg = static_cast<std::uint8_t>((reg << 1U) ^ kPolynomial);
}
else {
reg = static_cast<std::uint8_t>(reg << 1U);
}
}
}
return static_cast<std::uint8_t>(reg ^ kXorOut);
}
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();
return CP_SUCCESS;
}
void tactile_close(CPCodecContext* ctx) {
if (!ctx || !ctx->priv_data) {
return;
}
if (auto* priv = get_priv(ctx); priv != nullptr) {
priv->~TactileDecoderContext();
}
}
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();
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;
}
template<std::size_t N>
void keep_partial_start_prefix(std::vector<std::uint8_t>& buf, const std::array<std::uint8_t, N>& start_sequence) {
if (buf.empty() || N == 0U) {
return;
}
const std::size_t max_prefix = std::min<std::size_t>(N - 1U, buf.size());
for (std::size_t len = max_prefix; len > 0; --len) {
const auto seq_begin = start_sequence.begin();
const auto seq_end = seq_begin + static_cast<std::ptrdiff_t>(len);
const auto buf_begin =
buf.end() - static_cast<std::ptrdiff_t>(len);
if (std::equal(seq_begin, seq_end, buf_begin)) {
std::vector<std::uint8_t> tail(buf_begin, buf.end());
buf.swap(tail);
return;
}
}
buf.clear();
}
void trim_fifo_if_needed(std::vector<std::uint8_t>& buf, std::size_t max_fifo_bytes) {
if (buf.size() <= max_fifo_bytes) {
return;
}
const auto excess = buf.size() - max_fifo_bytes;
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(excess));
}
std::atomic<std::size_t>& expected_payload_bytes_for_tactile() {
static std::atomic<std::size_t> expected{kPiezoresistiveBPayloadSize};
return expected;
}
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();
storage->codec_id = ctx->codec ? ctx->codec->id : CPCodecID::Unknow;
if (storage->codec_id == CPCodecID::Tactile) {
const auto expected = expected_payload_bytes_for_tactile().load(std::memory_order_relaxed);
storage->update_limits(expected);
}
else {
storage->update_limits(kPiezoresistiveBPayloadSize);
}
return CP_SUCCESS;
}
void tactile_close(CPCodecContext* ctx) {
if (!ctx || !ctx->priv_data) {
return;
}
if (auto* priv = get_priv(ctx); priv != nullptr) {
priv->~TactileDecoderContext();
}
}
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();
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());
trim_fifo_if_needed(priv->fifo, priv->max_fifo_bytes);
}
if (packet.end_of_stream) {
priv->end_of_stream = true;
}
return CP_SUCCESS;
}
int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
auto* priv = get_priv(ctx);
if (!priv) {
@@ -111,10 +204,9 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
return CP_ERROR_EAGAIN;
}
const auto start_it = std::search(buf.begin(), buf.end(),
kStartSequence.begin(), kStartSequence.end());
const auto start_it = std::search(buf.begin(), buf.end(), kStartSequence.begin(), kStartSequence.end());
if (start_it == buf.end()) {
buf.clear();
keep_partial_start_prefix(buf, kStartSequence);
if (priv->end_of_stream) {
priv->end_of_stream = false;
return CP_ERROR_EOF;
@@ -142,8 +234,7 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
}
const std::uint16_t data_length =
static_cast<std::uint16_t>(data[2]) |
static_cast<std::uint16_t>(static_cast<std::uint16_t>(data[3]) << 8U);
static_cast<std::uint16_t>(data[2]) | static_cast<std::uint16_t>(static_cast<std::uint16_t>(data[3]) << 8U);
if (data_length < kFixedSectionSize) {
buf.erase(buf.begin());
@@ -151,6 +242,10 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
}
const std::size_t total_frame_length = kHeaderSize + static_cast<std::size_t>(data_length) + 1U;
if (total_frame_length > priv->max_frame_bytes) {
buf.erase(buf.begin());
continue;
}
if (buf.size() < total_frame_length) {
if (priv->end_of_stream) {
buf.clear();
@@ -160,129 +255,280 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
return CP_ERROR_EAGAIN;
}
const std::uint8_t computed_crc = crc8_with_xorout(data + kHeaderSize, data_length);
const std::uint8_t frame_crc = data[kHeaderSize + static_cast<std::size_t>(data_length)];
const auto crc_offset = total_frame_length - 1U;
const std::uint8_t computed_crc =
crc8_with_xorout(data, crc_offset); // header..last payload byte (excludes CRC)
const std::uint8_t frame_crc = data[crc_offset];
if (computed_crc != frame_crc) {
buf.erase(buf.begin());
continue;
}
frame.data.assign(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
frame.pts = priv->next_pts++;
frame.pts = priv->next_pts++;
frame.key_frame = true;
frame.valid = true;
frame.valid = true;
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
return CP_SUCCESS;
}
}
const CPCodec kTactileCodec {
.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
};
}
std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
return std::nullopt;
int tactile_b_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
auto* priv = get_priv(ctx);
if (!priv) {
return CP_ERROR_INVALID_STATE;
}
const auto* bytes = frame.data.data();
const std::size_t size = frame.data.size();
auto& buf = priv->fifo;
while (true) {
if (buf.size() < kPiezoresistiveBStartSequence.size()) {
if (priv->end_of_stream) {
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
const auto start_it = std::search(buf.begin(),
buf.end(),
kPiezoresistiveBStartSequence.begin(),
kPiezoresistiveBStartSequence.end());
if (start_it == buf.end()) {
keep_partial_start_prefix(buf, kPiezoresistiveBStartSequence);
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() < kPiezoresistiveBFrameSize) {
if (priv->end_of_stream) {
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
const auto end_offset = kPiezoresistiveBFrameSize - kPiezoresistiveBEndSequence.size();
const auto end_it = buf.begin() + static_cast<std::ptrdiff_t>(end_offset);
if (!std::equal(end_it,
end_it + static_cast<std::ptrdiff_t>(kPiezoresistiveBEndSequence.size()),
kPiezoresistiveBEndSequence.begin())) {
buf.erase(buf.begin());
continue;
}
frame.data.assign(buf.begin(),
buf.begin() + static_cast<std::ptrdiff_t>(kPiezoresistiveBFrameSize));
frame.pts = priv->next_pts++;
frame.key_frame = true;
frame.valid = true;
buf.erase(buf.begin(),
buf.begin() + static_cast<std::ptrdiff_t>(kPiezoresistiveBFrameSize));
return CP_SUCCESS;
}
}
const CPCodec kTactileCodec{
.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
};
const CPCodec kTactileBCodec{
.name = "tactile_serial_b",
.long_name = "Piezoresistive B tactile serial protocol decoder",
.type = CPMediaType::Data,
.id = CPCodecID::PiezoresistiveB,
.priv_data_size = sizeof(TactileDecoderContext),
.init = &tactile_init,
.close = &tactile_close,
.send_packet = &tactile_send_packet,
.receive_frame = &tactile_b_receive_frame
};
} // namespace
std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
// if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
// return std::nullopt;
// }
std::cout << "frame valid:" << frame.valid << ", frame.data.size:" << frame.data.size() << std::endl;
const auto* bytes = frame.data.data();
const std::size_t size = frame.data.size();
if (bytes[0] != kStartByteFirst || bytes[1] != kStartByteSecond) {
return std::nullopt;
}
std::cout << "frame valid1:" << frame.valid << ", frame.data.size1:" << frame.data.size() << std::endl;
const std::uint16_t data_length =
static_cast<std::uint16_t>(bytes[2]) |
static_cast<std::uint16_t>(static_cast<std::uint16_t>(bytes[3]) << 8U);
static_cast<std::uint16_t>(bytes[2]) | static_cast<std::uint16_t>(static_cast<std::uint16_t>(bytes[3]) << 8U);
qDebug() << "data_length: " << data_length;
if (data_length < kFixedSectionSize) {
return std::nullopt;
}
std::cout << "frame valid2:" << frame.valid << ", frame.data.size1:" << frame.data.size() << std::endl;
const std::size_t expected_size = kHeaderSize + static_cast<std::size_t>(data_length) + 1U;
if (size != expected_size) {
return std::nullopt;
}
const std::uint8_t crc_byte = bytes[size - 1U];
const std::uint8_t computed_crc = crc8_with_xorout(bytes + kHeaderSize, data_length);
std::cout << "frame valid3:" << frame.valid << ", frame.data.size1:" << frame.data.size() << std::endl;
const std::uint8_t crc_byte = bytes[expected_size - 1U];
const std::uint8_t computed_crc =
crc8_with_xorout(bytes, expected_size - 1U); // header..last payload byte
if (computed_crc != crc_byte) {
return std::nullopt;
}
const std::uint8_t device_address = bytes[4];
const std::uint8_t reserved = bytes[5];
const std::uint8_t response_function = bytes[6];
std::cout << "frame valid4:" << frame.valid << ", frame.data.size1:" << frame.data.size() << std::endl;
const std::uint8_t device_address = bytes[4];
const std::uint8_t reserved = bytes[5];
const std::uint8_t response_function = bytes[6];
const std::uint32_t start_address =
static_cast<std::uint32_t>(bytes[7]) |
(static_cast<std::uint32_t>(bytes[8]) << 8U) |
(static_cast<std::uint32_t>(bytes[9]) << 16U) |
(static_cast<std::uint32_t>(bytes[10]) << 24U);
static_cast<std::uint32_t>(bytes[7]) | (static_cast<std::uint32_t>(bytes[8]) << 8U) | (static_cast<std::uint32_t>(bytes[9]) << 16U) | (static_cast<std::uint32_t>(bytes[10]) << 24U);
const std::uint16_t return_byte_count =
static_cast<std::uint16_t>(bytes[11]) |
(static_cast<std::uint16_t>(bytes[12]) << 8U);
static_cast<std::uint16_t>(bytes[11]) | (static_cast<std::uint16_t>(bytes[12]) << 8U);
const std::uint8_t status = bytes[13];
const std::size_t payload_offset = kHeaderSize + kFixedSectionSize;
const std::size_t payload_length = static_cast<std::size_t>(data_length) - kFixedSectionSize;
if (payload_length != return_byte_count) {
const std::size_t payload_available =
data_length > kFixedSectionSize ? static_cast<std::size_t>(data_length) - kFixedSectionSize : 0U;
const std::size_t requested_payload = static_cast<std::size_t>(return_byte_count);
if (payload_available < requested_payload) {
return std::nullopt;
}
std::cout << "frame valid5:" << frame.valid << ", frame.data.size1:" << frame.data.size() << std::endl;
TactileFrame parsed{};
parsed.device_address = device_address;
parsed.reserved = reserved;
parsed.device_address = device_address;
parsed.reserved = reserved;
parsed.response_function = response_function;
parsed.function = static_cast<FunctionCode>(response_function & 0x7FU);
parsed.start_address = start_address;
parsed.function = static_cast<FunctionCode>(response_function & 0x7FU);
parsed.start_address = start_address;
parsed.return_byte_count = return_byte_count;
parsed.status = status;
parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + payload_length);
parsed.status = status;
parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + requested_payload);
return parsed;
}
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
if (frame.payload.size() != frame.return_byte_count) {
std::cout << "parse_pressure_values" << std::endl;
const auto requested_bytes = static_cast<std::size_t>(frame.return_byte_count);
const auto usable_bytes = std::min(requested_bytes, frame.payload.size());
if (usable_bytes == 0U || (usable_bytes % 2U != 0U)) {
return {};
}
if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
std::vector<std::uint16_t> values;
values.reserve(usable_bytes / 2U);
for (std::size_t idx = 0; idx + 1U < usable_bytes; 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::vector<std::uint16_t> parse_piezoresistive_b_pressures(const CPFrame& frame) {
// if (!frame.valid) {
// return {};
// }
// if (frame.data.size() != kPiezoresistiveBFrameSize) {
// return {};
// }
// if (frame.data.size() < kPiezoresistiveBFrameSize) {
// return {};
// }
// if (frame.data[0] != kPiezoresistiveBStartByteFirst || frame.data[1] != kPiezoresistiveBStartByteSecond) {
// return {};
// }
const auto end_offset = kPiezoresistiveBFrameSize - kPiezoresistiveBEndSequence.size();
// if (frame.data[end_offset] != kPiezoresistiveBEndByteFirst || frame.data[end_offset + 1U] != kPiezoresistiveBEndByteSecond) {
// return {};
// }
std::vector<std::uint16_t> values;
values.reserve(kPiezoresistiveBValueCount);
std::cout << "valuessize:" << values.size() << std::endl;
const auto payload_offset = kPiezoresistiveBStartSequence.size();
for (std::size_t idx = 0; idx < kPiezoresistiveBValueCount; ++idx) {
const auto base = payload_offset + idx * 2U;
if (base + 1U >= frame.data.size()) {
break;
}
const auto hi = static_cast<std::uint16_t>(frame.data[base]);
const auto lo = static_cast<std::uint16_t>(frame.data[base + 1U]);
values.push_back(static_cast<std::uint16_t>((hi << 8U) | lo));
}
return values;
}
std::vector<std::uint8_t> extract_piezoresistive_b_payload(const CPFrame& frame) {
if (!frame.valid) {
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;
}
const CPCodec* tactile_codec() {
return &kTactileCodec;
}
void register_tactile_codec() {
cpcodec_register(&kTactileCodec);
}
}
if (frame.data.size() != kPiezoresistiveBFrameSize) {
return {};
}
if (frame.data[0] != kPiezoresistiveBStartByteFirst || frame.data[1] != kPiezoresistiveBStartByteSecond) {
return {};
}
const auto payload_offset = kPiezoresistiveBStartSequence.size();
const auto payload_end = payload_offset + kPiezoresistiveBPayloadSize;
if (frame.data.size() < payload_end + kPiezoresistiveBEndSequence.size()) {
return {};
}
if (frame.data[payload_end] != kPiezoresistiveBEndByteFirst || frame.data[payload_end + 1U] != kPiezoresistiveBEndByteSecond) {
return {};
}
return std::vector<std::uint8_t>(
frame.data.begin() + static_cast<std::ptrdiff_t>(payload_offset),
frame.data.begin() + static_cast<std::ptrdiff_t>(payload_end));
}
void set_tactile_expected_payload_bytes(std::size_t bytes) {
const auto clamped = std::min<std::size_t>(
std::max<std::size_t>(bytes, 2U),
kAbsoluteMaxPayloadBytes);
expected_payload_bytes_for_tactile().store(clamped, std::memory_order_relaxed);
}
const CPCodec* tactile_codec() {
return &kTactileCodec;
}
void register_tactile_codec() {
cpcodec_register(&kTactileCodec);
}
const CPCodec* tactile_b_codec() {
return &kTactileBCodec;
}
void register_tactile_b_codec() {
cpcodec_register(&kTactileBCodec);
}
} // namespace ffmsep::tactile

11
components/ffmsep/tactile/tacdec.hh
View File

@@ -8,6 +8,11 @@
namespace ffmsep::tactile {
inline constexpr std::uint8_t kStartByteFirst = 0xAA;
inline constexpr std::uint8_t kStartByteSecond = 0x55;
inline constexpr std::uint8_t kPiezoresistiveBStartByteFirst = 0xF0;
inline constexpr std::uint8_t kPiezoresistiveBStartByteSecond = 0xF1;
inline constexpr std::uint8_t kPiezoresistiveBEndByteFirst = 0xF1;
inline constexpr std::uint8_t kPiezoresistiveBEndByteSecond = 0xF0;
inline constexpr std::size_t kPiezoresistiveBValueCount = 200;
enum class FunctionCode : std::uint8_t {
Unknown = 0x00,
@@ -40,7 +45,13 @@ 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_patrix_coordinate_payload(const TactileFrame &frame);
std::vector<std::uint16_t> parse_piezoresistive_b_pressures(const CPFrame &frame);
std::vector<std::uint8_t> extract_piezoresistive_b_payload(const CPFrame &frame);
// 配置触觉 A 类型预期的 payload 字节数（点数 * 2），用于限制解码 FIFO。
void set_tactile_expected_payload_bytes(std::size_t bytes);
const CPCodec *tactile_codec();
void register_tactile_codec();
const CPCodec *tactile_b_codec();
void register_tactile_b_codec();
} // namespace ffmsep::tactile