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

242
components/ffmsep/cpdecoder.hh
View File

@@ -1,125 +1,119 @@
#pragma once
#include "components/ffmsep/cpdecoder.hh"
#include <cstdint>
#include <cstddef>
#include <mutex>
#include <optional>
#include <string>
#include <string_view>
#include <vector>
#include <initializer_list>
namespace ffmsep {
inline constexpr int CP_SUCCESS = 0;
inline constexpr int CP_ERROR_EOF = -1;
inline constexpr int CP_ERROR_EAGAIN = -2;
inline constexpr int CP_ERROR_NOT_OPEN = -3;
inline constexpr int CP_ERROR_INVALID_STATE = -4;
inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;
enum class CPMediaType : std::uint8_t {
Unknow = 0,
Data,
};
enum class CPCodecID : std::uint32_t {
Unknow = 0,
Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识 Tactile Quick Codec Type
};
struct CPPacket {
std::vector<std::uint8_t> payload;
std::int64_t pts = 0;
std::int64_t dts = 0;
bool end_of_stream = false;
bool flush = false;
CPPacket() = default;
CPPacket(std::vector<std::uint8_t> data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept
: payload(std::move(data)), pts(pts_value), dts(dts_value) {}
[[nodiscard]] bool empty() const noexcept {return payload.empty();}
};
struct CPFrame {
std::vector<std::uint8_t> data;
std::int64_t pts = 0;
bool key_frame = false;
bool valid = false;
void reset() noexcept {
data.clear();
key_frame = false;
valid = false;
pts = 0;
}
};
struct CPCodecContext;
struct CPCodec {
using InitFn = int(*)(CPCodecContext*);
using CloseFn = void(*)(CPCodecContext*);
using SendPacketFn = int(*)(CPCodecContext*, const CPPacket&);
using ReceiveFrameFn = int(*)(CPCodecContext*, CPFrame&);
const char* name = nullptr;
const char* long_name = nullptr;
CPMediaType type = CPMediaType::Unknow;
CPCodecID id = CPCodecID::Unknow;
std::size_t priv_data_size = 0;
InitFn init = nullptr;
CloseFn close = nullptr;
SendPacketFn send_packet = nullptr;
ReceiveFrameFn receive_frame = nullptr;
};
struct CPCodecContext {
const CPCodec* codec = nullptr;
void* priv_data = nullptr;
CPMediaType codec_type = CPMediaType::Unknow;
bool is_open = false;
void clear() noexcept {
codec = nullptr;
priv_data = nullptr;
codec_type = CPMediaType::Unknow;
is_open = false;
priv_storage.clear();
}
void* ensure_priv_storage(std::size_t size);
void release_priv_storage() noexcept;
template<typename T>
[[nodiscard]] T* priv_as() noexcept {
return static_cast<T*>(priv_data);
}
template<typename T>
[[nodiscard]] const T* priv_as() const noexcept {
return static_cast<const T*>(priv_data);
}
private:
std::vector<std::uint8_t> priv_storage;
friend CPCodecContext* cpcodec_alloc_context(const CPCodec*);
friend int cpcodec_open(CPCodecContext*, const CPCodec*);
friend int cpcodec_close(CPCodecContext*);
};
void cpcodec_register(const CPCodec* codec);
void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs);
const CPCodec* cpcodec_find_decoder(CPCodecID id);
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
std::vector<const CPCodec*> cpcodec_list_codecs();
CPCodecContext* cpcodec_alloc_context(const CPCodec* codec);
int cpcodec_open(CPCodecContext*, const CPCodec*);
int cpcodec_close(CPCodecContext*);
void cpcodec_free_context(CPCodecContext **ctx);
int cpcodec_send_packet(CPCodecContext*, const CPPacket*);
int cpcodec_receive_frame(CPCodecContext*, CPFrame*);
#pragma once
#include <cstdint>
#include <cstddef>
#include <string_view>
#include <vector>
#include <initializer_list>
namespace ffmsep {
inline constexpr int CP_SUCCESS = 0;
inline constexpr int CP_ERROR_EOF = -1;
inline constexpr int CP_ERROR_EAGAIN = -2;
inline constexpr int CP_ERROR_NOT_OPEN = -3;
inline constexpr int CP_ERROR_INVALID_STATE = -4;
inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;
enum class CPMediaType : std::uint8_t {
Unknow = 0,
Data,
};
enum class CPCodecID : std::uint32_t {
Unknow = 0,
Tactile = 0x54514354u // 'T','Q','C','T'：触觉传感器协议标识 Tactile Quick Codec Type
};
struct CPPacket {
std::vector<std::uint8_t> payload;
std::int64_t pts = 0;
std::int64_t dts = 0;
bool end_of_stream = false;
bool flush = false;
CPPacket() = default;
CPPacket(std::vector<std::uint8_t> data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept
: payload(std::move(data)), pts(pts_value), dts(dts_value) {}
[[nodiscard]] bool empty() const noexcept {return payload.empty();}
};
struct CPFrame {
std::vector<std::uint8_t> data;
std::int64_t pts = 0;
bool key_frame = false;
bool valid = false;
void reset() noexcept {
data.clear();
key_frame = false;
valid = false;
pts = 0;
}
};
struct CPCodecContext;
struct CPCodec {
using InitFn = int(*)(CPCodecContext*);
using CloseFn = void(*)(CPCodecContext*);
using SendPacketFn = int(*)(CPCodecContext*, const CPPacket&);
using ReceiveFrameFn = int(*)(CPCodecContext*, CPFrame&);
const char* name = nullptr;
const char* long_name = nullptr;
CPMediaType type = CPMediaType::Unknow;
CPCodecID id = CPCodecID::Unknow;
std::size_t priv_data_size = 0;
InitFn init = nullptr;
CloseFn close = nullptr;
SendPacketFn send_packet = nullptr;
ReceiveFrameFn receive_frame = nullptr;
};
struct CPCodecContext {
const CPCodec* codec = nullptr;
void* priv_data = nullptr;
CPMediaType codec_type = CPMediaType::Unknow;
bool is_open = false;
void clear() noexcept {
codec = nullptr;
priv_data = nullptr;
codec_type = CPMediaType::Unknow;
is_open = false;
priv_storage.clear();
}
void* ensure_priv_storage(std::size_t size);
void release_priv_storage() noexcept;
template<typename T>
[[nodiscard]] T* priv_as() noexcept {
return static_cast<T*>(priv_data);
}
template<typename T>
[[nodiscard]] const T* priv_as() const noexcept {
return static_cast<const T*>(priv_data);
}
private:
std::vector<std::uint8_t> priv_storage;
friend CPCodecContext* cpcodec_alloc_context(const CPCodec*);
friend int cpcodec_open(CPCodecContext*, const CPCodec*);
friend int cpcodec_close(CPCodecContext*);
};
void cpcodec_register(const CPCodec* codec);
void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs);
const CPCodec* cpcodec_find_decoder(CPCodecID id);
const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
std::vector<const CPCodec*> cpcodec_list_codecs();
CPCodecContext* cpcodec_alloc_context(const CPCodec* codec);
int cpcodec_open(CPCodecContext*, const CPCodec*);
int cpcodec_close(CPCodecContext*);
void cpcodec_free_context(CPCodecContext **ctx);
int cpcodec_send_packet(CPCodecContext*, const CPPacket*);
int cpcodec_receive_frame(CPCodecContext*, CPFrame*);
}

50
components/ffmsep/cpstream_core.cc
View File

@@ -62,6 +62,9 @@ struct CPStreamCore::Impl {
if (config_.frame_queue_capacity == 0U) {
config_.frame_queue_capacity = 1U;
}
if (config_.slave_request_interval.count() < 0) {
config_.slave_request_interval = std::chrono::milliseconds{0};
}
frame_queue_capacity_ = config_.frame_queue_capacity;
}
@@ -106,7 +109,9 @@ struct CPStreamCore::Impl {
config_.parity,
config_.stopbits,
config_.flowcontrol);
serial->open();
if (!serial->isOpen()) {
serial->open();
}
serial->flush();
{
@@ -213,6 +218,9 @@ struct CPStreamCore::Impl {
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);
}
return true;
}
@@ -227,6 +235,9 @@ struct CPStreamCore::Impl {
if (reader_thread_.joinable()) {
reader_thread_.join();
}
if (slave_thread_.joinable()) {
slave_thread_.join();
}
signal_decoder_flush(true);
packet_cv_.notify_all();
@@ -399,6 +410,33 @@ struct CPStreamCore::Impl {
}
}
void slave_loop() {
const auto command = config_.slave_request_command;
auto interval = config_.slave_request_interval;
if (interval.count() < 0) {
interval = std::chrono::milliseconds{0};
}
const bool repeat = interval.count() > 0;
while (!stop_requested_.load(std::memory_order_acquire)) {
const bool success = send(command);
if (!success) {
std::this_thread::sleep_for(kReaderIdleSleep);
continue;
}
if (!repeat) {
break;
}
auto remaining = interval;
while (remaining.count() > 0 && !stop_requested_.load(std::memory_order_acquire)) {
const auto step = std::min(remaining, kReaderIdleSleep);
std::this_thread::sleep_for(step);
remaining -= step;
}
}
}
void decoder_loop() {
while (true) {
Packet packet;
@@ -450,6 +488,15 @@ struct CPStreamCore::Impl {
decoded.pts = frame.pts;
decoded.received_at = std::chrono::steady_clock::now();
decoded.frame = std::move(frame);
if (codec_descriptor_ && codec_descriptor_->id == CPCodecID::Tactile) {
if (auto parsed = tactile::parse_frame(decoded.frame)) {
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;
}
}
}
FrameCallback callback_copy;
{
@@ -520,6 +567,7 @@ struct CPStreamCore::Impl {
CPCodecContext* codec_ctx_ = nullptr;
std::thread reader_thread_;
std::thread slave_thread_;
std::thread decoder_thread_;
std::mutex packet_mutex_;

6
components/ffmsep/cpstream_core.hh
View File

@@ -1,6 +1,7 @@
#pragma once
#include "components/ffmsep/cpdecoder.hh"
#include "components/ffmsep/tactile/tacdec.hh"
#include <chrono>
#include <cstdint>
#include <functional>
@@ -16,6 +17,9 @@ struct DecodedFrame {
CPFrame frame;
std::chrono::steady_clock::time_point received_at{};
std::int64_t pts = 0;
std::optional<tactile::TactileFrame> tactile;
std::vector<std::uint16_t> tactile_pressures;
std::optional<tactile::MatrixSize> tactile_matrix_size;
};
struct CPStreamConfig {
@@ -31,6 +35,8 @@ struct CPStreamConfig {
std::size_t frame_queue_capacity = 16;
CPCodecID codec_id = CPCodecID::Unknow;
std::string codec_name;
std::vector<std::uint8_t> slave_request_command{};
std::chrono::milliseconds slave_request_interval{200};
};
class CPStreamCore {

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);
}
}

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

@@ -1,42 +1,45 @@
#pragma once
#include "cpdecoder.hh"
#include <cstdint>
#include <optional>
#include <vector>
#pragma once
#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;
inline constexpr std::uint8_t kStartByteFirst = 0xAA;
inline constexpr std::uint8_t kStartByteSecond = 0x55;
enum class FunctionCode : std::uint8_t {
Unknown = 0x00,
ReadMatrix = 0x01,
ReadSingle = 0x02,
ReadTemperature = 0x03,
SetDeviceId = 0x51,
SetMatrixSize = 0x52,
CalibrationMode = 0x53,
};
struct MatrixSize {
std::uint8_t long_edge = 0;
std::uint8_t short_edge = 0;
};
ReadTemperature = 0x03,
SetDeviceId = 0x51,
SetMatrixSize = 0x52,
CalibrationMode = 0x53,
};
struct MatrixSize {
std::uint8_t long_edge = 0;
std::uint8_t short_edge = 0;
};
struct TactileFrame {
std::uint8_t device_address = 0;
std::uint8_t reserved = 0;
std::uint8_t response_function = 0;
FunctionCode function = FunctionCode::Unknown;
std::uint8_t data_length = 0;
std::uint32_t start_address = 0;
std::uint16_t return_byte_count = 0;
std::uint8_t status = 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_patrix_coordinate_payload(const TactileFrame& frame);
const CPCodec* tactile_codec();
void register_tactile_codec();
}
std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
std::optional<MatrixSize> parse_patrix_coordinate_payload(const TactileFrame& frame);
const CPCodec* tactile_codec();
void register_tactile_codec();
}