Files
ts-qt/components/ffmsep/tactile/tacdec.cc

535 lines
19 KiB
C++

#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <array>
#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::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;
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) {
return buf.empty() ? nullptr : buf.data();
}
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 {
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;
}
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) {
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;
}
const auto start_it = std::search(buf.begin(), buf.end(), kStartSequence.begin(), kStartSequence.end());
if (start_it == buf.end()) {
keep_partial_start_prefix(buf, kStartSequence);
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() < kHeaderSize) {
if (priv->end_of_stream) {
buf.clear();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
const std::uint8_t* data = buffer_data(buf);
if (!data) {
buf.clear();
continue;
}
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);
if (data_length < kFixedSectionSize) {
buf.erase(buf.begin());
continue;
}
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();
priv->end_of_stream = false;
return CP_ERROR_EOF;
}
return CP_ERROR_EAGAIN;
}
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.key_frame = true;
frame.valid = true;
buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
return CP_SUCCESS;
}
}
int tactile_b_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.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);
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;
}
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;
}
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);
const std::uint16_t return_byte_count =
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_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.response_function = response_function;
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 + requested_payload);
return parsed;
}
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
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 {};
}
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 {};
}
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