121 lines
3.7 KiB
C++
121 lines
3.7 KiB
C++
#include <algorithm>
|
|
#include <chrono>
|
|
#include <csignal>
|
|
#include <iostream>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include "components/ffmsep/cpstream_core.hh"
|
|
#include "components/ffmsep/tactile/tacdec.hh"
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
static volatile std::sig_atomic_t g_running = 1;
|
|
|
|
void handle_sigint(int) {
|
|
g_running = 0;
|
|
}
|
|
|
|
int main(int argc, char** argv) {
|
|
std::signal(SIGINT, handle_sigint);
|
|
|
|
// Register tactile codec so the stream can resolve it by ID or name.
|
|
ffmsep::tactile::register_tactile_codec();
|
|
|
|
const auto ports = ffmsep::CPStreamCore::list_available_ports();
|
|
const auto describe_ports = [&ports]() {
|
|
if (ports.empty()) {
|
|
std::cout << "No serial ports detected.\n";
|
|
return;
|
|
}
|
|
std::cout << "Available serial ports:\n";
|
|
for (const auto& p : ports) {
|
|
std::cout << " - " << p.port.c_str() << " (" << p.description.c_str() << ")\n";
|
|
}
|
|
};
|
|
|
|
std::string port;
|
|
if (argc >= 2) {
|
|
const std::string requested = argv[1];
|
|
if (ports.empty()) {
|
|
std::cout << "No serial ports detected, attempting requested '" << requested << "'.\n";
|
|
port = requested;
|
|
} else {
|
|
const auto it = std::find_if(ports.begin(), ports.end(),
|
|
[&](const auto& info) { return info.port == requested; });
|
|
if (it == ports.end()) {
|
|
std::cerr << "Requested port '" << requested << "' not found among detected ports.\n";
|
|
describe_ports();
|
|
return 1;
|
|
}
|
|
port = it->port;
|
|
}
|
|
} else {
|
|
describe_ports();
|
|
if (!ports.empty()) {
|
|
port = ports.front().port;
|
|
std::cout << "Auto-selecting: " << port << "\n";
|
|
} else {
|
|
std::cerr << "No serial ports found. Exiting.\n";
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
ffmsep::CPStreamConfig cfg;
|
|
cfg.port = port;
|
|
cfg.baudrate = 115200;
|
|
cfg.codec_id = ffmsep::CPCodecID::Tactile; // resolve tactile decoder
|
|
cfg.read_chunk_size = 256;
|
|
cfg.packet_queue_capacity = 128;
|
|
cfg.frame_queue_capacity = 32;
|
|
|
|
ffmsep::CPStreamCore core(cfg);
|
|
if (!core.open()) {
|
|
std::cerr << "Open failed: " << core.last_error() << "\n";
|
|
return 2;
|
|
}
|
|
|
|
// Optional: receive frames via callback
|
|
core.set_frame_callback([](const ffmsep::DecodedFrame& df) {
|
|
const auto maybe = ffmsep::tactile::parse_frame(df.frame);
|
|
if (!maybe) {
|
|
return;
|
|
}
|
|
const auto& tf = *maybe;
|
|
const auto pv = ffmsep::tactile::parse_pressure_values(tf);
|
|
std::cout << "Frame pts=" << df.pts
|
|
<< " addr=" << int(tf.device_address)
|
|
<< " func=" << int(static_cast<std::uint8_t>(tf.function))
|
|
<< " len=" << int(tf.data_length)
|
|
<< " pressures=" << pv.size() << "\n";
|
|
});
|
|
|
|
if (!core.start()) {
|
|
std::cerr << "Start failed: " << core.last_error() << "\n";
|
|
return 3;
|
|
}
|
|
|
|
std::cout << "Streaming from " << port << ". Press Ctrl+C to stop...\n";
|
|
|
|
// Also demonstrate polling API (in case users don't want callbacks)
|
|
while (g_running) {
|
|
ffmsep::DecodedFrame df;
|
|
if (core.wait_for_frame(df, 200ms)) {
|
|
const auto maybe = ffmsep::tactile::parse_frame(df.frame);
|
|
if (maybe) {
|
|
const auto sz = ffmsep::tactile::parse_matrix_size_payload(*maybe);
|
|
if (sz) {
|
|
std::cout << " matrix=" << int(sz->long_edge) << "x" << int(sz->short_edge) << "\n";
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
core.stop();
|
|
core.close();
|
|
std::cout << "Stopped.\n";
|
|
return 0;
|
|
}
|