498 lines
20 KiB
C++
498 lines
20 KiB
C++
#include "fgc/Application.h"
|
|
|
|
#include "fgc/CaptureScheduler.h"
|
|
#include "fgc/CommandParser.h"
|
|
#include "fgc/DumpParser.h"
|
|
#include "fgc/HelpText.h"
|
|
#include "fgc/ICameraSource.h"
|
|
#include "fgc/IControlChannel.h"
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/ImagePipeline.h"
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/ScanGrid.h"
|
|
#include "fgc/SerialMotorController.h"
|
|
#include "fgc/mock/MockCameraSource.h"
|
|
#include "fgc/mock/MockMotorController.h"
|
|
#include "fgc/mock/NullControlChannel.h"
|
|
#include "fgc/ui/HeadlessUi.h"
|
|
#include "fgc/ui/IUserInterface.h"
|
|
#include "fgc/ui/UiSnapshot.h"
|
|
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <queue>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <thread>
|
|
|
|
#if FGC_WITH_MQTT
|
|
#include "fgc/MqttControlChannel.h"
|
|
#endif
|
|
#if FGC_WITH_VIMBA
|
|
#include "fgc/VimbaCameraSource.h"
|
|
#endif
|
|
#if FGC_WITH_TUI
|
|
#include "fgc/ui/TuiUi.h"
|
|
#endif
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
// Human-readable list of the enabled wire-trace categories, for echoing back.
|
|
std::string traceNames(unsigned mask) {
|
|
if (mask == 0) return "none";
|
|
std::string out;
|
|
const std::pair<LogCat, const char*> all[] = {
|
|
{LogCat::Serial, "serial"}, {LogCat::Mqtt, "mqtt"},
|
|
{LogCat::Camera, "camera"}, {LogCat::Control, "control"},
|
|
};
|
|
for (const auto& [cat, name] : all) {
|
|
if (mask & static_cast<unsigned>(cat)) {
|
|
if (!out.empty()) out += ',';
|
|
out += name;
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct Application::Impl {
|
|
Impl(AppConfig c, RuntimeOptions o) : cfg(std::move(c)), opts(std::move(o)) {}
|
|
|
|
AppConfig cfg;
|
|
RuntimeOptions opts;
|
|
|
|
std::unique_ptr<IControlChannel> channel;
|
|
std::unique_ptr<IMotorController> motor;
|
|
std::unique_ptr<ICameraSource> camera;
|
|
std::unique_ptr<ImagePipeline> pipeline;
|
|
std::unique_ptr<CaptureScheduler> scheduler;
|
|
std::unique_ptr<IUserInterface> ui;
|
|
ScanGrid grid; // outlives scheduler (holds a reference to it)
|
|
|
|
std::atomic<bool> running{true};
|
|
std::mutex cmd_mutex;
|
|
std::queue<std::string> cmd_queue;
|
|
|
|
std::chrono::steady_clock::time_point start_time;
|
|
std::mutex snapshot_mutex;
|
|
UiSnapshot latest_snapshot;
|
|
|
|
std::string statusTopicTower() const { return cfg.general.tower_name; }
|
|
|
|
std::unique_ptr<IControlChannel> makeChannel() {
|
|
bool want = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
|
|
#if FGC_WITH_MQTT
|
|
if (want) {
|
|
return std::make_unique<MqttControlChannel>(cfg.network.broker_ip,
|
|
cfg.general.tower_name,
|
|
cfg.network.mqtt_user,
|
|
cfg.network.mqtt_pw);
|
|
}
|
|
#else
|
|
if (want) LOG_WARN << "MQTT requested but binary built without MQTT; using null channel";
|
|
#endif
|
|
return std::make_unique<NullControlChannel>();
|
|
}
|
|
|
|
std::unique_ptr<IMotorController> makeMotor() {
|
|
bool mock = opts.mock_serial.value_or(cfg.features.mock_serial);
|
|
if (mock) return std::make_unique<MockMotorController>();
|
|
return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
|
|
}
|
|
|
|
std::unique_ptr<ICameraSource> makeCamera() {
|
|
bool mock = opts.mock_camera.value_or(cfg.features.mock_camera);
|
|
#if !FGC_WITH_VIMBA
|
|
if (!mock) {
|
|
LOG_WARN << "Camera requested but binary built without Vimba X; using mock camera";
|
|
mock = true;
|
|
}
|
|
#endif
|
|
if (mock) {
|
|
int count = cfg.camera.ids.empty() ? 1 : static_cast<int>(cfg.camera.ids.size());
|
|
return std::make_unique<MockCameraSource>(count);
|
|
}
|
|
#if FGC_WITH_VIMBA
|
|
return std::make_unique<VimbaCameraSource>(cfg.camera.ids);
|
|
#else
|
|
return std::make_unique<MockCameraSource>(1);
|
|
#endif
|
|
}
|
|
|
|
std::unique_ptr<IUserInterface> makeUi() {
|
|
bool want = opts.use_tui.value_or(cfg.ui.enable_tui);
|
|
#if FGC_WITH_TUI
|
|
if (want) return std::make_unique<TuiUi>();
|
|
#else
|
|
if (want) LOG_WARN << "TUI requested but binary built without TUI support; using headless console";
|
|
#endif
|
|
return std::make_unique<HeadlessUi>();
|
|
}
|
|
|
|
// Read live state (on the control thread) into a plain snapshot the UI can
|
|
// copy and render without touching the logic objects. The log pane is filled
|
|
// by the UI from its own ring buffer, so it is left empty here.
|
|
UiSnapshot buildSnapshot() const {
|
|
UiSnapshot s;
|
|
|
|
const bool mock_serial = opts.mock_serial.value_or(cfg.features.mock_serial);
|
|
s.header.tower = cfg.general.tower_name;
|
|
s.header.build = "dev";
|
|
s.header.uptime_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::steady_clock::now() - start_time).count();
|
|
s.header.live = !mock_serial;
|
|
|
|
// --- Gimbal ---
|
|
MotorTelemetry t = motor->telemetry();
|
|
s.gimbal.present = motor->connected();
|
|
s.gimbal.pitch_present = t.pitch_present;
|
|
auto fillAxis = [](AxisView& v, const char* label, const AxisTelemetry& a,
|
|
const AxisMap& map, long target_counts) {
|
|
v.label = label;
|
|
v.state = a.state;
|
|
v.deg = map.toDeg(a.xenc);
|
|
v.xactual = a.xactual;
|
|
v.xenc = a.xenc;
|
|
v.target_deg = map.toDeg(target_counts);
|
|
v.sg = a.sg;
|
|
v.cs = a.cs;
|
|
v.pwm = a.pwm;
|
|
v.drv_status = a.drv_status;
|
|
v.moving = a.moving();
|
|
v.standstill = a.standstill;
|
|
v.stall = a.stall;
|
|
v.overtemp = a.overtemp;
|
|
v.endstop_l = a.endstop_l;
|
|
v.endstop_r = a.endstop_r;
|
|
};
|
|
long yaw_tgt = scheduler ? scheduler->yawTargetCounts() : 0;
|
|
long pitch_tgt = scheduler ? scheduler->pitchTargetCounts() : 0;
|
|
fillAxis(s.gimbal.yaw, "YAW", t.yaw, cfg.geometry.yaw, yaw_tgt);
|
|
fillAxis(s.gimbal.pitch, "PITCH", t.pitch, cfg.geometry.pitch, pitch_tgt);
|
|
|
|
// Homing travel limits come from the last firmware dump (lim_neg/lim_pos);
|
|
// convert the endstop counts to degrees via the per-axis geometry.
|
|
DumpData dd = parseDump(motor->lastDump());
|
|
if (dd.valid) {
|
|
auto fillLimits = [](AxisView& v, const DumpAxis& a, const AxisMap& map) {
|
|
v.has_limits = true;
|
|
v.lim_neg = a.lim_neg;
|
|
v.lim_pos = a.lim_pos;
|
|
v.lim_neg_deg = map.toDeg(a.lim_neg);
|
|
v.lim_pos_deg = map.toDeg(a.lim_pos);
|
|
};
|
|
for (const auto& a : dd.axes) {
|
|
if (a.axis == 'Y') fillLimits(s.gimbal.yaw, a, cfg.geometry.yaw);
|
|
else if (a.axis == 'P') fillLimits(s.gimbal.pitch, a, cfg.geometry.pitch);
|
|
}
|
|
}
|
|
|
|
// --- Sensors (DHT11 + MTi not integrated yet) ---
|
|
s.sensors = pendingSensorsView();
|
|
|
|
// --- Camera / capture ---
|
|
s.capture.present = true;
|
|
s.capture.active = scheduler && scheduler->captureActive();
|
|
s.capture.image_rate = scheduler ? scheduler->imageRate() : 0.0;
|
|
s.capture.camera_count = camera ? camera->cameraCount() : 0;
|
|
s.capture.labels = cfg.camera.labels;
|
|
if (pipeline) {
|
|
if (auto ev = pipeline->lastEvent()) {
|
|
s.capture.has_last = true;
|
|
s.capture.last_label = ev->camera;
|
|
s.capture.last_heading_deg = ev->heading_decideg / 10.0;
|
|
s.capture.last_pitch_deg = ev->pitch_decideg / 10.0;
|
|
s.capture.last_ts_ms = ev->timestamp_ms;
|
|
}
|
|
}
|
|
|
|
// --- Connectivity ---
|
|
s.conn.mqtt_enabled = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
|
|
s.conn.mqtt_connected = channel && channel->connected();
|
|
s.conn.broker = cfg.network.broker_ip;
|
|
s.conn.tower = cfg.general.tower_name;
|
|
s.conn.control_code = scheduler ? scheduler->controlCode() : 0;
|
|
s.conn.target_heading = scheduler ? scheduler->targetHeading() : "0";
|
|
s.conn.last_status_code = s.conn.control_code; // echoed back as status
|
|
|
|
// --- Diagnostics (last firmware DUMP) ---
|
|
s.dump.text = motor->lastDump();
|
|
s.dump.has = !s.dump.text.empty();
|
|
return s;
|
|
}
|
|
|
|
void publishSnapshot() {
|
|
UiSnapshot s = buildSnapshot();
|
|
std::lock_guard<std::mutex> lock(snapshot_mutex);
|
|
latest_snapshot = std::move(s);
|
|
}
|
|
|
|
void runInitSequence() {
|
|
using namespace std::chrono_literals;
|
|
LOG_INFO << "Running gimbal init sequence (enable + home)";
|
|
motor->sendCommand("ENABLE Y");
|
|
motor->sendCommand("ENABLE P");
|
|
motor->sendCommand("HOME"); // homes all axes; firmware runs it non-blocking
|
|
|
|
auto allReady = [](const MotorTelemetry& t) {
|
|
return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
|
|
};
|
|
|
|
// The axes may already be READY (EEPROM fast-path), so first wait briefly
|
|
// for homing to actually begin (an axis leaves READY) before waiting for
|
|
// it to finish - otherwise we'd see the residual READY and return early.
|
|
const auto t0 = std::chrono::steady_clock::now();
|
|
while (running && std::chrono::steady_clock::now() < t0 + 3s) {
|
|
std::this_thread::sleep_for(100ms);
|
|
publishSnapshot(); // keep the TUI's gimbal panel live during homing
|
|
if (!allReady(motor->telemetry())) break; // homing started
|
|
}
|
|
|
|
// Wait for completion (both axes READY again) or the homing timeout.
|
|
const auto deadline = std::chrono::steady_clock::now() + 65s;
|
|
while (running && std::chrono::steady_clock::now() < deadline) {
|
|
std::this_thread::sleep_for(250ms);
|
|
publishSnapshot(); // keep the TUI's gimbal panel live during homing
|
|
MotorTelemetry t = motor->telemetry();
|
|
if (t.yaw.state == AxisState::Error ||
|
|
(t.pitch_present && t.pitch.state == AxisState::Error)) {
|
|
LOG_WARN << "Homing reported ERROR state; check the gimbal";
|
|
break;
|
|
}
|
|
if (allReady(t)) {
|
|
LOG_INFO << "Gimbal homed (both axes READY)";
|
|
break;
|
|
}
|
|
}
|
|
// Production move speeds for subsequent MOVE commands.
|
|
motor->sendCommand("SPEED Y 50000");
|
|
motor->sendCommand("SPEED P 150000");
|
|
LOG_INFO << "Gimbal init finished";
|
|
}
|
|
|
|
void startCapture() {
|
|
camera->start();
|
|
scheduler->setCaptureActive(true);
|
|
LOG_INFO << "Capture started";
|
|
}
|
|
void stopCapture() {
|
|
camera->stop();
|
|
scheduler->setCaptureActive(false);
|
|
LOG_INFO << "Capture stopped";
|
|
}
|
|
|
|
// `trace off|none`, `trace all`, or `trace <cat> [on|off]` — toggle the
|
|
// verbatim wire-trace categories (serial/mqtt/camera/control).
|
|
void handleTrace(const Command& c) {
|
|
if (c.device.empty() || c.device == "off" || c.device == "none") {
|
|
Logger::setCategories(0);
|
|
} else if (c.device == "all") {
|
|
Logger::setCategories(static_cast<unsigned>(LogCat::All));
|
|
} else {
|
|
bool ok = true;
|
|
LogCat cat = Logger::catFromString(c.device, &ok);
|
|
if (!ok) {
|
|
LOG_WARN << "unknown trace category: " << c.device;
|
|
return;
|
|
}
|
|
bool on = (c.option != "off"); // "trace serial" / "trace serial on" => enable
|
|
if (on) Logger::enableCategory(cat);
|
|
else Logger::disableCategory(cat);
|
|
}
|
|
LOG_INFO << "trace categories: " << traceNames(Logger::categories());
|
|
}
|
|
|
|
// `goto <yaw_deg> <pitch_deg>` — aim the gimbal at an absolute heading and
|
|
// elevation in degrees. Converts to encoder counts via the operator-calibrated
|
|
// Geometry maps (which soft-clamp to the travel limits) and issues a two-axis
|
|
// MOVE so both axes start together.
|
|
void handleGoto(const std::string& line) {
|
|
std::istringstream iss(line);
|
|
std::string verb;
|
|
double yaw_deg = 0.0, pitch_deg = 0.0;
|
|
if (!(iss >> verb >> yaw_deg >> pitch_deg)) {
|
|
LOG_WARN << "usage: goto <yaw_deg> <pitch_deg> (e.g. goto 30 -10)";
|
|
return;
|
|
}
|
|
long yc = cfg.geometry.yaw.toCounts(yaw_deg);
|
|
long pc = cfg.geometry.pitch.toCounts(pitch_deg);
|
|
LOG_INFO << "goto yaw=" << yaw_deg << "deg pitch=" << pitch_deg
|
|
<< "deg -> MOVE " << yc << "," << pc;
|
|
motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
|
|
}
|
|
|
|
void handleCommand(const std::string& line) {
|
|
Command c = parseCommand(line);
|
|
if (c.empty()) return;
|
|
LOG_TRACE_CAT(LogCat::Control) << "cmd " << line;
|
|
|
|
if (c.verb == "help") {
|
|
// c.device holds the optional topic (first token after "help").
|
|
for (const auto& l : renderHelp(c.device)) LOG_INFO << l;
|
|
} else if (c.verb == "goto") {
|
|
handleGoto(line);
|
|
} else if (c.verb == "dump") {
|
|
LOG_INFO << "requesting firmware dump...";
|
|
motor->sendCommand("DUMP");
|
|
} else if (c.verb == "exit") {
|
|
running = false;
|
|
} else if (c.verb == "start") {
|
|
startCapture();
|
|
} else if (c.verb == "stop") {
|
|
stopCapture();
|
|
} else if (c.verb == "debug") {
|
|
bool on = Logger::level() != LogLevel::Debug;
|
|
Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
|
|
LOG_INFO << "debug logging " << (on ? "on" : "off");
|
|
} else if (c.verb == "trace") {
|
|
handleTrace(c);
|
|
} else if (c.verb == "set") {
|
|
if (c.device == "camera") {
|
|
if (c.option == "fps" && c.has_value) camera->setFrameRate(c.value);
|
|
else if (c.option == "jxlq" && c.has_value) pipeline->setDistance(c.value);
|
|
else if (c.option == "jxle" && c.has_value) pipeline->setEffort(static_cast<int>(c.value));
|
|
else if (c.option == "display") pipeline->setDisplay(c.value != 0.0);
|
|
else LOG_WARN << "unknown 'set camera' option: " << c.option;
|
|
} else if (c.device == "fps" && c.has_value) {
|
|
scheduler->setImageRate(c.value);
|
|
LOG_INFO << "capture rate set to " << c.value << " img/s";
|
|
} else if (c.device == "motorctl") {
|
|
motor->sendCommand(c.option);
|
|
} else {
|
|
LOG_WARN << "unknown 'set' target: " << c.device;
|
|
}
|
|
} else {
|
|
LOG_WARN << "unknown command: " << c.verb;
|
|
}
|
|
}
|
|
|
|
void drainCommands() {
|
|
std::queue<std::string> local;
|
|
{
|
|
std::lock_guard<std::mutex> lock(cmd_mutex);
|
|
std::swap(local, cmd_queue);
|
|
}
|
|
while (!local.empty()) {
|
|
handleCommand(local.front());
|
|
local.pop();
|
|
}
|
|
}
|
|
|
|
int run() {
|
|
start_time = std::chrono::steady_clock::now();
|
|
|
|
// Log level: config first, CLI overrides.
|
|
if (!cfg.logging.level.empty() && !Logger::setLevelFromString(cfg.logging.level))
|
|
LOG_WARN << "unknown Logging.level '" << cfg.logging.level << "', keeping default";
|
|
if (!opts.log_level.empty() && !Logger::setLevelFromString(opts.log_level))
|
|
LOG_WARN << "unknown log level '" << opts.log_level << "', keeping default";
|
|
|
|
// Wire-trace categories: config first, CLI overrides (replaces, not merges).
|
|
if (!cfg.logging.trace.empty()) {
|
|
bool ok = true;
|
|
unsigned m = Logger::parseCategories(cfg.logging.trace, &ok);
|
|
if (!ok) LOG_WARN << "unknown trace category in Logging.trace '" << cfg.logging.trace << "'";
|
|
Logger::setCategories(m);
|
|
}
|
|
if (!opts.trace_categories.empty()) {
|
|
bool ok = true;
|
|
unsigned m = Logger::parseCategories(opts.trace_categories, &ok);
|
|
if (!ok) LOG_WARN << "unknown trace category in '" << opts.trace_categories << "'";
|
|
Logger::setCategories(m);
|
|
}
|
|
if (Logger::categories())
|
|
LOG_INFO << "wire trace enabled: " << traceNames(Logger::categories());
|
|
|
|
channel = makeChannel();
|
|
motor = makeMotor();
|
|
camera = makeCamera();
|
|
|
|
if (!channel->connect())
|
|
LOG_WARN << "Control channel not connected; continuing in degraded mode";
|
|
|
|
ImagePipeline::Params pp;
|
|
pp.output_dir = cfg.paths.output_dir;
|
|
pp.labels = cfg.camera.labels;
|
|
pp.tower = cfg.general.tower_name;
|
|
pp.demo = opts.demo;
|
|
// Orientation supplier: convert the per-axis encoder counts to degrees.
|
|
pipeline = std::make_unique<ImagePipeline>(
|
|
*channel,
|
|
[this] {
|
|
MotorTelemetry t = motor->telemetry();
|
|
return Orientation{static_cast<float>(cfg.geometry.yaw.toDeg(t.yaw.xenc)),
|
|
static_cast<float>(cfg.geometry.pitch.toDeg(t.pitch.xenc))};
|
|
},
|
|
pp);
|
|
|
|
ImagePipeline* pipe_ptr = pipeline.get();
|
|
camera->setFrameCallback([pipe_ptr](const Frame& f) { pipe_ptr->submit(f); });
|
|
|
|
try {
|
|
grid = ScanGrid::fromConfig(cfg.scan);
|
|
} catch (const std::exception& e) {
|
|
LOG_WARN << "scan grid load failed (" << e.what() << "); auto-sweep disabled";
|
|
}
|
|
LOG_INFO << "scan grid: " << grid.size() << " waypoints";
|
|
scheduler = std::make_unique<CaptureScheduler>(*motor, *camera, *channel, cfg.image_rate(),
|
|
cfg.geometry, grid);
|
|
|
|
motor->start();
|
|
camera->open();
|
|
pipeline->start();
|
|
channel->publishStatus(0);
|
|
|
|
// Seed an initial snapshot so the UI's first frame is populated, then
|
|
// start the interface (headless console or TUI) on its own thread. The
|
|
// UI is a pure observer: it pulls snapshots and pushes command strings
|
|
// into the same queue the console always used.
|
|
//
|
|
// The UI is brought up BEFORE the init/home sequence so the dashboard is
|
|
// visible during homing (which can take many seconds) and so the serial
|
|
// wire traces render in the TUI's log pane instead of scrolling raw —
|
|
// makeUi()/start() installs the TUI log sink, so anything logged after
|
|
// this point is captured by it.
|
|
publishSnapshot();
|
|
ui = makeUi();
|
|
ui->start([this] { std::lock_guard<std::mutex> lk(snapshot_mutex); return latest_snapshot; },
|
|
[this](const std::string& line) {
|
|
std::lock_guard<std::mutex> lk(cmd_mutex);
|
|
cmd_queue.push(line);
|
|
});
|
|
|
|
if (opts.init) runInitSequence();
|
|
if (opts.start) startCapture();
|
|
|
|
LOG_INFO << "Entering control loop (type 'exit' to quit)";
|
|
while (running) {
|
|
drainCommands();
|
|
scheduler->tick();
|
|
publishSnapshot();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
LOG_INFO << "Shutting down";
|
|
if (ui) ui->stop();
|
|
pipeline->stop();
|
|
camera->stop();
|
|
camera->close();
|
|
motor->stop();
|
|
channel->disconnect();
|
|
return 0;
|
|
}
|
|
};
|
|
|
|
Application::Application(AppConfig config, RuntimeOptions options)
|
|
: impl_(std::make_unique<Impl>(std::move(config), std::move(options))) {}
|
|
|
|
Application::~Application() = default;
|
|
|
|
int Application::run() { return impl_->run(); }
|
|
|
|
} // namespace fgc
|