285 lines
11 KiB
C++
285 lines
11 KiB
C++
#include "fgc/MtiImuSource.h"
|
|
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/MtiProtocol.h"
|
|
|
|
#include <array>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <functional>
|
|
#include <mutex>
|
|
#include <thread>
|
|
|
|
#include <boost/asio.hpp>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
|
|
namespace fgc {
|
|
|
|
struct MtiImuSource::Impl {
|
|
Impl(std::string dev, unsigned int b)
|
|
: device(std::move(dev)), baud(b), serial(io),
|
|
framer([this](uint8_t mid, const uint8_t* d, std::size_t n) { onFrame(mid, d, n); }) {}
|
|
|
|
using clock = std::chrono::steady_clock;
|
|
|
|
std::string device;
|
|
unsigned int baud;
|
|
boost::asio::io_context io;
|
|
boost::asio::serial_port serial;
|
|
std::thread io_thread;
|
|
std::array<uint8_t, 256> rxbuf{};
|
|
|
|
std::mutex mutex;
|
|
ImuSample latest;
|
|
clock::time_point last_rx{};
|
|
std::atomic<bool> open{false};
|
|
|
|
ImuDeviceConfig cfg; // device config read back at startup
|
|
bool have_cfg = false;
|
|
|
|
MtiFramer framer;
|
|
unsigned bad_len_warned = 0;
|
|
|
|
void onFrame(uint8_t mid, const uint8_t* d, std::size_t n) {
|
|
if (mid != kMidMTData) return; // ignore acks/other during streaming
|
|
auto s = parseMTData(mid, d, n);
|
|
if (!s) {
|
|
if (bad_len_warned++ < 1)
|
|
LOG_WARN << "MTi: unexpected MTData length " << n << " (expected "
|
|
<< int(kMTDataLen) << "); device may not be in the configured mode";
|
|
return;
|
|
}
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
latest = *s;
|
|
last_rx = clock::now();
|
|
}
|
|
|
|
void doRead() {
|
|
serial.async_read_some(
|
|
boost::asio::buffer(rxbuf),
|
|
[this](const boost::system::error_code& ec, std::size_t n) {
|
|
if (ec) {
|
|
if (ec != boost::asio::error::operation_aborted)
|
|
LOG_WARN << "MTi serial read failed: " << ec.message();
|
|
return;
|
|
}
|
|
framer.feed(rxbuf.data(), n);
|
|
doRead();
|
|
});
|
|
}
|
|
|
|
// Synchronous, best-effort config handshake (no ack parsing): runs on the
|
|
// calling thread BEFORE the io_thread starts, so there is no concurrent
|
|
// access to the serial port. Small delays let the device switch states.
|
|
// If set_profile >= 0, also selects that XKF profile while in Config state
|
|
// (persists to non-volatile memory).
|
|
void configure(int set_profile = -1) {
|
|
using namespace std::chrono_literals;
|
|
auto write = [this](const std::vector<uint8_t>& m) {
|
|
boost::system::error_code ec;
|
|
boost::asio::write(serial, boost::asio::buffer(m), ec);
|
|
if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
|
|
};
|
|
write(msgGoToConfig()); std::this_thread::sleep_for(60ms);
|
|
if (set_profile >= 0) {
|
|
write(msgSetFilterProfile(static_cast<uint16_t>(set_profile)));
|
|
std::this_thread::sleep_for(60ms);
|
|
}
|
|
write(msgSetOutputMode()); std::this_thread::sleep_for(60ms);
|
|
write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms);
|
|
// Read back the device configuration while still in Config State.
|
|
queryConfig(write);
|
|
write(msgGoToMeasurement()); std::this_thread::sleep_for(60ms);
|
|
// Drop any pre-config (old-format) bytes so the framer starts clean.
|
|
::tcflush(serial.native_handle(), TCIFLUSH);
|
|
}
|
|
|
|
// Pause the streaming io_thread, re-run the handshake (optionally setting an
|
|
// XKF profile), then resume streaming. Cancels + drains the outstanding read
|
|
// first so it can't steal the config-ack bytes during the re-query. Called
|
|
// from the control thread (no concurrent serial access). Used by refreshConfig
|
|
// and setFilterProfile.
|
|
void restartWithConfig(int set_profile) {
|
|
io.stop();
|
|
if (io_thread.joinable()) io_thread.join();
|
|
io.restart();
|
|
boost::system::error_code ec;
|
|
serial.cancel(ec);
|
|
static_cast<void>(io.run()); // run the aborted read handler (no reschedule)
|
|
io.restart();
|
|
configure(set_profile);
|
|
doRead();
|
|
io_thread = std::thread([this] { io.run(); });
|
|
}
|
|
|
|
// Query the device configuration in Config State and decode the acks. Runs on
|
|
// the calling thread before the streaming io_thread starts; it drives the
|
|
// io_context itself with run_for() and leaves it clean (restarted, all
|
|
// pending ops cancelled) so the later io.run() streams normally.
|
|
template <class WriteFn>
|
|
void queryConfig(WriteFn&& write) {
|
|
using namespace std::chrono_literals;
|
|
ImuDeviceConfig local;
|
|
MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) {
|
|
applyImuConfigAck(local, mid, d, n);
|
|
});
|
|
|
|
// Drop any buffered pre-config stream (e.g. MTData from before GoToConfig
|
|
// took effect) so the read window below is dominated by the query acks.
|
|
::tcflush(serial.native_handle(), TCIFLUSH);
|
|
|
|
write(msgReqProductCode());
|
|
write(msgReqDID());
|
|
write(msgReqFWRev());
|
|
write(msgReqPeriod());
|
|
write(msgReqOutputMode());
|
|
write(msgReqOutputSettings());
|
|
write(msgReqFilterProfile());
|
|
write(msgReqAvailFilterProfiles());
|
|
|
|
std::array<uint8_t, 256> buf{};
|
|
std::function<void()> rd = [&]() {
|
|
serial.async_read_some(boost::asio::buffer(buf),
|
|
[&](const boost::system::error_code& ec, std::size_t n) {
|
|
if (ec) return; // cancelled / error: stop
|
|
cf.feed(buf.data(), n);
|
|
rd();
|
|
});
|
|
};
|
|
io.restart();
|
|
rd();
|
|
static_cast<void>(io.run_for(500ms)); // collect acks for up to half a second
|
|
// Drain the pending read so its by-ref handler can't fire later with the
|
|
// local buffer/framer already destroyed.
|
|
boost::system::error_code ec;
|
|
serial.cancel(ec);
|
|
io.restart();
|
|
static_cast<void>(io.run());
|
|
io.restart();
|
|
|
|
finalizeImuConfig(local);
|
|
if (local.valid) {
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
cfg = local;
|
|
have_cfg = true;
|
|
LOG_INFO << "MTi config: " << (local.product_code.empty() ? "?" : local.product_code)
|
|
<< " fw " << (local.firmware.empty() ? "?" : local.firmware)
|
|
<< " scenario " << int(local.scenario_type)
|
|
<< (local.scenario_label.empty() ? "" : " (" + local.scenario_label + ")")
|
|
<< " @ " << local.sample_rate_hz << " Hz";
|
|
} else {
|
|
LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)";
|
|
}
|
|
}
|
|
};
|
|
|
|
MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
|
|
: impl_(std::make_unique<Impl>(std::move(device), baud)) {}
|
|
|
|
MtiImuSource::~MtiImuSource() { stop(); }
|
|
|
|
void MtiImuSource::start() {
|
|
namespace asio = boost::asio;
|
|
boost::system::error_code ec;
|
|
impl_->serial.open(impl_->device, ec);
|
|
if (ec) {
|
|
LOG_ERROR << "MTi: failed to open " << impl_->device << ": " << ec.message();
|
|
return;
|
|
}
|
|
impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
|
|
impl_->serial.set_option(asio::serial_port_base::character_size(8));
|
|
impl_->serial.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
|
|
impl_->serial.set_option(
|
|
asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
|
|
impl_->serial.set_option(
|
|
asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
|
|
|
|
impl_->open = true;
|
|
impl_->configure();
|
|
impl_->doRead();
|
|
impl_->io_thread = std::thread([this] { impl_->io.run(); });
|
|
LOG_INFO << "MTi IMU started on " << impl_->device << " @ " << impl_->baud;
|
|
}
|
|
|
|
void MtiImuSource::stop() {
|
|
if (!impl_) return;
|
|
impl_->io.stop();
|
|
if (impl_->io_thread.joinable()) impl_->io_thread.join();
|
|
boost::system::error_code ec;
|
|
if (impl_->serial.is_open()) impl_->serial.close(ec);
|
|
impl_->open = false;
|
|
}
|
|
|
|
bool MtiImuSource::connected() const {
|
|
if (!impl_->open) return false;
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest.valid &&
|
|
(Impl::clock::now() - impl_->last_rx) < std::chrono::seconds(1);
|
|
}
|
|
|
|
std::optional<ImuSample> MtiImuSource::sample() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
if (!impl_->latest.valid ||
|
|
(Impl::clock::now() - impl_->last_rx) >= std::chrono::seconds(1))
|
|
return std::nullopt;
|
|
return impl_->latest;
|
|
}
|
|
|
|
std::optional<ImuDeviceConfig> MtiImuSource::config() const {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
if (!impl_->have_cfg) return std::nullopt;
|
|
return impl_->cfg;
|
|
}
|
|
|
|
// Re-read the device configuration: briefly pause the streaming io_thread,
|
|
// re-run the Config-state handshake (which re-queries product/firmware/output/
|
|
// XKF profile and re-applies our output settings), then resume streaming. Mirrors
|
|
// start() minus opening the port. Called from the control thread; sample()/config()
|
|
// are called from that same thread when building the snapshot, so there is no
|
|
// concurrent serial access during the refresh.
|
|
void MtiImuSource::refreshConfig() {
|
|
if (!impl_->open) { LOG_WARN << "MTi: refresh ignored (not started)"; return; }
|
|
LOG_INFO << "MTi: refreshing configuration (briefly pausing the stream)...";
|
|
impl_->restartWithConfig(-1); // re-query only, no profile change
|
|
}
|
|
|
|
bool MtiImuSource::setFilterProfile(int type) {
|
|
if (!impl_->open) { LOG_WARN << "MTi: setFilterProfile ignored (not started)"; return false; }
|
|
if (type < 0) return false;
|
|
LOG_INFO << "MTi: setting XKF profile to type " << type << " (persists on device)...";
|
|
impl_->restartWithConfig(type);
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->have_cfg && impl_->cfg.has_scenario && impl_->cfg.scenario_type == type;
|
|
}
|
|
|
|
// Send a command while the streaming io_thread is running. The write is posted
|
|
// onto the io_context so it runs on the io_thread, never concurrently with the
|
|
// in-flight async_read (concurrent ops on one serial_port are unsafe — same rule
|
|
// as SerialMotorController).
|
|
void MtiImuSource::noRotation(int seconds) {
|
|
if (!impl_->open) { LOG_WARN << "MTi: noRotation ignored (not started)"; return; }
|
|
if (seconds < 1) seconds = 1;
|
|
auto msg = std::make_shared<std::vector<uint8_t>>(
|
|
msgSetNoRotation(static_cast<uint16_t>(seconds)));
|
|
boost::asio::post(impl_->io, [this, msg] {
|
|
boost::system::error_code ec;
|
|
boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
|
|
if (ec) LOG_WARN << "MTi noRotation write failed: " << ec.message();
|
|
});
|
|
LOG_INFO << "MTi: no-rotation update for " << seconds << " s (hold still)";
|
|
}
|
|
|
|
void MtiImuSource::headingReset() {
|
|
if (!impl_->open) { LOG_WARN << "MTi: headingReset ignored (not started)"; return; }
|
|
auto msg = std::make_shared<std::vector<uint8_t>>(msgResetOrientation(kResetHeading));
|
|
boost::asio::post(impl_->io, [this, msg] {
|
|
boost::system::error_code ec;
|
|
boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
|
|
if (ec) LOG_WARN << "MTi headingReset write failed: " << ec.message();
|
|
});
|
|
LOG_INFO << "MTi: heading reset (current direction is now yaw 0)";
|
|
}
|
|
|
|
} // namespace fgc
|