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

feat:data slove and update heatmap

Browse Source
This commit is contained in:
lenn
2025-10-29 14:09:28 +08:00
parent c50b44efe2
commit c6cef3d89d
200 changed files with 100674 additions and 52814 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,25 +1,26 @@
#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <vector>
#include <array>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <vector>
namespace ffmsep::tactile {
namespace {
constexpr std::size_t kMinimumFrameSize = 1
+ 1
+ 1
+ 1
+ 0
+ 2
+ 2;
constexpr std::uint16_t kCrcInitial = 0xFFFF;
constexpr std::uint16_t kCrcPolynomial = 0xA001;
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
};
struct TactileDecoderContext {
std::vector<std::uint8_t> fifo;
@@ -27,77 +28,72 @@ 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;
std::uint8_t crc8_with_xorout(const std::uint8_t* data, std::size_t length) {
std::uint8_t reg = kCrcInitial;
for (std::size_t i = 0; i < length; ++i) {
crc ^= static_cast<std::uint16_t>(data[i]);
reg ^= data[i];
for (int bit = 0; bit < 8; ++bit) {
if ((crc & 0x0001U) != 0U) {
crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial);
}
else {
crc = static_cast<std::uint16_t>(crc >> 1U);
if ((reg & 0x80U) != 0U) {
reg = static_cast<std::uint8_t>((reg << 1U) ^ kCrcPolynomial);
} else {
reg = static_cast<std::uint8_t>(reg << 1U);
}
}
}
return crc;
return static_cast<std::uint8_t>(reg ^ kCrcXorOut);
}
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;
}
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;
}
int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
auto* priv = get_priv(ctx);
if (!priv) {
@@ -115,7 +111,8 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
return CP_ERROR_EAGAIN;
}
auto start_it = std::find(buf.begin(), buf.end(), kStartByte);
const auto start_it = std::search(buf.begin(), buf.end(),
kStartSequence.begin(), kStartSequence.end());
if (start_it == buf.end()) {
buf.clear();
if (priv->end_of_stream) {
@@ -129,7 +126,7 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
buf.erase(buf.begin(), start_it);
}
if (buf.size() < kMinimumFrameSize) {
if (buf.size() < kHeaderSize) {
if (priv->end_of_stream) {
buf.clear();
priv->end_of_stream = false;
@@ -139,11 +136,21 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
}
const std::uint8_t* data = buffer_data(buf);
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 (!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 (buf.size() < total_frame_length) {
if (priv->end_of_stream) {
buf.clear();
@@ -153,33 +160,13 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
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;
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) {
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)];
if (computed_crc != frame_crc) {
buf.erase(buf.begin());
continue;
}
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());
continue;
}
(void)address;
(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;
@@ -189,20 +176,20 @@ int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
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 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;
@@ -210,64 +197,92 @@ std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
const auto* bytes = frame.data.data();
const std::size_t size = frame.data.size();
if (bytes[0] != kStartByte) {
if (bytes[0] != kStartByteFirst || bytes[1] != kStartByteSecond) {
return std::nullopt;
}
if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) {
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);
if (data_length < kFixedSectionSize) {
return std::nullopt;
}
if (size < 4U) {
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 length = bytes[3];
if (frame_length_from_payload(length) != size) {
const std::uint8_t crc_byte = bytes[size - 1U];
const std::uint8_t computed_crc = crc8_with_xorout(bytes + kHeaderSize, data_length);
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];
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_length = static_cast<std::size_t>(data_length) - kFixedSectionSize;
if (payload_length != return_byte_count) {
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);
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 + payload_length);
return parsed;
}
std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
if (frame.payload.size() != frame.return_byte_count) {
return {};
}
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;
}
const CPCodec* tactile_codec() {
return &kTactileCodec;
}
void register_tactile_codec() {
cpcodec_register(&kTactileCodec);
}
}
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);
}
}