Add logging, I/O interfaces, and extracted capture scheduler

CMakeLists.txt

@@ -43,6 +43,9 @@ pkg_check_modules(JXL REQUIRED IMPORTED_TARGET libjxl libjxl_threads)
 add_library(fgc_core STATIC
   src/core/Config.cpp
   src/core/Paths.cpp
+  src/core/Logger.cpp
+  src/core/TelemetryParser.cpp
+  src/core/CaptureScheduler.cpp
   ini.c
 )
 target_include_directories(fgc_core PUBLIC

Log.h

@@ -1 +1,3 @@
 #pragma once
+// Legacy shim: the project's logging now lives in fgc/Logger.h.
+#include "fgc/Logger.h"

include/fgc/CaptureScheduler.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "fgc/ICameraSource.h"
+#include "fgc/IControlChannel.h"
+#include "fgc/IMotorController.h"
+
+#include <functional>
+#include <string>
+
+namespace fgc {
+
+// The capture control loop, extracted from the old main() while-loop and
+// expressed against the I/O interfaces so it can be unit-tested with mocks.
+//
+// Each tick():
+//   1. polls the control channel (updates control code / target heading,
+//      echoes the code back as status),
+//   2. reads motor telemetry,
+//   3. runs the capture cycle: when the configured interval elapses, stops /
+//      points the gimbal, then software-triggers the cameras.
+//
+// ControlCode 0 = automatic sweep ("p"); ControlCode 1 = drive to the
+// MQTT-supplied target heading ("kd<heading>").
+class CaptureScheduler {
+public:
+    // now_ms: monotonic millisecond clock; defaults to steady_clock. Injectable
+    // for deterministic tests.
+    CaptureScheduler(IMotorController& motor, ICameraSource& camera,
+                     IControlChannel& channel, double image_rate,
+                     std::function<long long()> now_ms = {});
+
+    void setCaptureActive(bool active);  // mirrors the old cam_started flag
+    bool captureActive() const { return capture_active_; }
+
+    void   setImageRate(double rate);    // images per second
+    double imageRate() const { return image_rate_; }
+
+    // Run one iteration of the control logic.
+    void tick();
+
+    // Inspection (mainly for tests).
+    int         controlCode() const { return control_code_; }
+    std::string targetHeading() const { return target_heading_; }
+
+private:
+    long long elapsedMs() const;
+    void      resetTimer();
+
+    IMotorController& motor_;
+    ICameraSource&    camera_;
+    IControlChannel&  channel_;
+
+    std::function<long long()> now_ms_;
+    double      image_rate_;
+    long long   timer_start_ = 0;
+
+    bool        capture_active_ = false;
+    int         control_code_ = 0;
+    std::string target_heading_ = "0";
+    bool        trigger_after_stopping_ = false;
+};
+
+}  // namespace fgc

include/fgc/ICameraSource.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <cstdint>
+#include <functional>
+#include <vector>
+
+namespace fgc {
+
+// One captured frame, owning a copy of its pixel buffer (was image_store_8bit).
+struct Frame {
+    std::vector<uint8_t> data;
+    uint32_t  width  = 0;
+    uint32_t  height = 0;
+    int       channels = 0;     // 1 (mono) or 3 (RGB)
+    long long timestamp_ms = 0; // Unix epoch ms
+    int       cam_id = 0;       // index into the configured camera list
+};
+
+// Abstraction over the camera array. Implemented by VimbaCameraSource (Allied
+// Vision Vimba X) and MockCameraSource (synthetic frames, no hardware).
+//
+// Completed frames are delivered to the callback set via setFrameCallback();
+// the consumer (ImagePipeline) encodes and stores them.
+class ICameraSource {
+public:
+    using FrameCallback = std::function<void(const Frame&)>;
+
+    virtual ~ICameraSource() = default;
+
+    virtual void open()  = 0;   // acquire/open cameras
+    virtual void close() = 0;
+    virtual void start() = 0;   // begin acquisition
+    virtual void stop()  = 0;
+
+    // Software-trigger a capture. Returns true on success.
+    virtual bool trigger() = 0;
+
+    virtual void setFrameCallback(FrameCallback cb) = 0;
+
+    // Number of cameras this source manages.
+    virtual int cameraCount() const = 0;
+};
+
+}  // namespace fgc

include/fgc/IControlChannel.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <string>
+
+namespace fgc {
+
+// Remote control input (was struct mqtt_sub_data). The *_available flags mark
+// whether a fresh value arrived since the last poll().
+struct ControlCommand {
+    bool        control_code_available = false;
+    int         control_code = 0;   // 0 = automatic sweep, 1 = directed to target heading
+    bool        heading_available = false;
+    std::string target_heading;     // kept as string; forwarded as "kd<heading>"
+};
+
+// Notification published after an image is captured and saved.
+struct CamEvent {
+    std::string tower;          // tower / FWT name
+    std::string camera;         // "RGB" / "ACR" / "NIR"
+    int         heading_decideg = 0;  // heading * 10 (one decimal as integer)
+    long long   timestamp_ms = 0;     // Unix epoch ms; matches the image filename
+};
+
+// Abstraction over the remote control/telemetry channel. Implemented by
+// MqttControlChannel (Eclipse Paho) and NullControlChannel (no broker; used
+// for development - publishes are dropped and poll() yields a default).
+class IControlChannel {
+public:
+    virtual ~IControlChannel() = default;
+
+    // Returns true once usable. NullControlChannel always succeeds.
+    virtual bool connect()    = 0;
+    virtual void disconnect() = 0;
+    virtual bool connected() const = 0;
+
+    virtual void publishStatus(int code)               = 0;
+    virtual void publishCamEvent(const CamEvent& event) = 0;
+
+    // Latest control input; clears the *_available flags so each update is
+    // acted on once.
+    virtual ControlCommand poll() = 0;
+};
+
+}  // namespace fgc

include/fgc/IMotorController.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <string>
+
+namespace fgc {
+
+// Telemetry snapshot from the motor controller (was struct motor_info).
+struct MotorTelemetry {
+    int   encoder        = 0;   // Xenc
+    int   encoder_err    = 0;   // Xerr
+    int   sgt_val        = 0;   // StallGuard value
+    int   sgt_stat       = 0;   // StallGuard status
+    int   is_moving      = 0;   // movement flag (kept as int to preserve firmware semantics)
+    int   control_status = 0;   // driver/controller status
+    float heading        = 0.f; // degrees
+    int   deviation_warn = 0;
+    int   humidity       = 0;
+    int   temperature    = 0;
+    int   fan_pwm        = 0;   // 0-255
+};
+
+// Abstraction over the gimbal's motor controller. Implemented by
+// SerialMotorController (Boost.Asio serial link) and MockMotorController
+// (simulated, for development without hardware).
+class IMotorController {
+public:
+    virtual ~IMotorController() = default;
+
+    // Begin/*end* background telemetry reading.
+    virtual void start() = 0;
+    virtual void stop()  = 0;
+
+    // Send a raw command string to the controller (e.g. "p", "kd180").
+    virtual void sendCommand(const std::string& cmd) = 0;
+
+    // Latest telemetry snapshot (thread-safe in implementations).
+    virtual MotorTelemetry telemetry() = 0;
+
+    // Whether the underlying link is usable.
+    virtual bool connected() const = 0;
+};
+
+}  // namespace fgc

include/fgc/Logger.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <ostream>
+#include <sstream>
+#include <string>
+
+namespace fgc {
+
+enum class LogLevel { Trace = 0, Debug, Info, Warn, Error, Off };
+
+// Minimal leveled, thread-safe logger. Each log line is assembled in a
+// per-statement buffer and written atomically under a shared mutex, so lines
+// from different threads never interleave. Level filtering is global.
+//
+// Usage:   LOG_INFO << "starting, rate=" << rate;
+// Lines at Warn/Error go to stderr; everything else to stdout.
+class Logger {
+public:
+    static void     setLevel(LogLevel level);
+    static LogLevel level();
+    static bool     enabled(LogLevel level);
+
+    // Parse "trace"|"debug"|"info"|"warn"|"error"|"off" (case-insensitive).
+    // Returns false (and leaves the level unchanged) on an unknown string.
+    static bool setLevelFromString(const std::string& s);
+};
+
+// RAII helper that buffers one log line and flushes it on commit().
+// Designed to be driven by the LOG_* macros' for-loop guard.
+class LogStream {
+public:
+    explicit LogStream(LogLevel level);
+    bool          pending() const { return enabled_ && !done_; }
+    void          commit();
+    std::ostream& stream() { return buffer_; }
+
+private:
+    LogLevel           level_;
+    bool               enabled_;
+    bool               done_ = false;
+    std::ostringstream buffer_;
+};
+
+}  // namespace fgc
+
+// Brace-safe: the whole macro is a single for-statement, so it composes
+// correctly inside if/else without dangling-else hazards, and the message
+// expression is never evaluated when the level is disabled.
+#define FGC_LOG_AT(level)                                       \
+    for (::fgc::LogStream _fgc_ls(level); _fgc_ls.pending(); _fgc_ls.commit()) \
+    _fgc_ls.stream()
+
+#define LOG_TRACE FGC_LOG_AT(::fgc::LogLevel::Trace)
+#define LOG_DEBUG FGC_LOG_AT(::fgc::LogLevel::Debug)
+#define LOG_INFO  FGC_LOG_AT(::fgc::LogLevel::Info)
+#define LOG_WARN  FGC_LOG_AT(::fgc::LogLevel::Warn)
+#define LOG_ERROR FGC_LOG_AT(::fgc::LogLevel::Error)

include/fgc/TelemetryParser.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "fgc/IMotorController.h"
+
+#include <optional>
+#include <string>
+
+namespace fgc {
+
+// Parse a motor-controller telemetry line of the form:
+//
+//   $;Xenc;Xerr;sgt_val;sgt_stat;is_moving;control_status;hdg;deviation_warn;humid;temp;fan_pwm;
+//
+// i.e. a leading '$' marker followed by 11 ';'-separated numeric fields
+// (a trailing ';' is tolerated). Returns nullopt if the line does not start
+// with '$', has too few fields, or a field fails to convert.
+std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line);
+
+}  // namespace fgc

src/core/CaptureScheduler.cpp

@@ -0,0 +1,80 @@
+#include "fgc/CaptureScheduler.h"
+
+#include <chrono>
+
+namespace fgc {
+
+namespace {
+
+long long steadyNowMs() {
+    using namespace std::chrono;
+    return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
+}
+
+// Whether a software trigger should fire given the current telemetry.
+//
+// NOTE: this preserves the original behaviour of triggering while the gimbal
+// reports is_moving == 1. That looks counter-intuitive (one might expect to
+// trigger once stopped). See docs/known-issues.md #7 - kept as-is pending
+// confirmation of the firmware's is_moving semantics.
+bool shouldTrigger(const MotorTelemetry& t) { return t.is_moving == 1; }
+
+}  // namespace
+
+CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
+                                   IControlChannel& channel, double image_rate,
+                                   std::function<long long()> now_ms)
+    : motor_(motor),
+      camera_(camera),
+      channel_(channel),
+      now_ms_(now_ms ? std::move(now_ms) : steadyNowMs),
+      image_rate_(image_rate) {
+    timer_start_ = now_ms_();
+}
+
+void CaptureScheduler::setCaptureActive(bool active) { capture_active_ = active; }
+void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }
+
+long long CaptureScheduler::elapsedMs() const { return now_ms_() - timer_start_; }
+void      CaptureScheduler::resetTimer() { timer_start_ = now_ms_(); }
+
+void CaptureScheduler::tick() {
+    // 1. Remote control input.
+    ControlCommand cmd = channel_.poll();
+    if (cmd.control_code_available) {
+        control_code_ = cmd.control_code;
+        channel_.publishStatus(control_code_);
+    }
+    if (cmd.heading_available) {
+        target_heading_ = cmd.target_heading;
+    }
+
+    // 2. Telemetry.
+    MotorTelemetry t = motor_.telemetry();
+
+    // 3. Capture cycle. The interval gate is 1000/rate milliseconds.
+    if (control_code_ != 0 && control_code_ != 1) return;
+
+    const double interval_ms = image_rate_ > 0.0 ? 1000.0 / image_rate_ : 0.0;
+
+    if (capture_active_ && interval_ms > 0.0 && elapsedMs() > interval_ms) {
+        if (t.is_moving == 1) {
+            resetTimer();
+            trigger_after_stopping_ = true;
+            if (control_code_ == 0)
+                motor_.sendCommand("p");
+            else  // control_code_ == 1
+                motor_.sendCommand("kd" + target_heading_);
+        }
+    }
+
+    if (trigger_after_stopping_ && elapsedMs() > 100) {
+        if (shouldTrigger(t)) {
+            if (camera_.trigger()) {
+                trigger_after_stopping_ = false;
+            }
+        }
+    }
+}
+
+}  // namespace fgc

src/core/Logger.cpp

@@ -0,0 +1,73 @@
+#include "fgc/Logger.h"
+
+#include <atomic>
+#include <chrono>
+#include <cstdio>
+#include <ctime>
+#include <iostream>
+#include <mutex>
+
+namespace fgc {
+
+namespace {
+
+std::atomic<LogLevel> g_level{LogLevel::Info};
+std::mutex            g_mutex;
+
+const char* tag(LogLevel l) {
+    switch (l) {
+        case LogLevel::Trace: return "TRACE";
+        case LogLevel::Debug: return "DEBUG";
+        case LogLevel::Info:  return "INFO ";
+        case LogLevel::Warn:  return "WARN ";
+        case LogLevel::Error: return "ERROR";
+        case LogLevel::Off:   return "OFF  ";
+    }
+    return "?????";
+}
+
+std::string timestamp() {
+    using namespace std::chrono;
+    auto now = system_clock::now();
+    auto t   = system_clock::to_time_t(now);
+    auto ms  = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;
+
+    std::tm tm{};
+    localtime_r(&t, &tm);
+
+    char buf[32];
+    std::snprintf(buf, sizeof(buf), "%02d:%02d:%02d.%03d", tm.tm_hour, tm.tm_min,
+                  tm.tm_sec, static_cast<int>(ms.count()));
+    return buf;
+}
+
+}  // namespace
+
+void     Logger::setLevel(LogLevel level) { g_level.store(level); }
+LogLevel Logger::level() { return g_level.load(); }
+bool     Logger::enabled(LogLevel level) { return level >= g_level.load(); }
+
+bool Logger::setLevelFromString(const std::string& s) {
+    std::string v;
+    for (char c : s) v += static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
+    if (v == "trace") { setLevel(LogLevel::Trace); return true; }
+    if (v == "debug") { setLevel(LogLevel::Debug); return true; }
+    if (v == "info")  { setLevel(LogLevel::Info);  return true; }
+    if (v == "warn" || v == "warning") { setLevel(LogLevel::Warn); return true; }
+    if (v == "error") { setLevel(LogLevel::Error); return true; }
+    if (v == "off" || v == "none") { setLevel(LogLevel::Off); return true; }
+    return false;
+}
+
+LogStream::LogStream(LogLevel level) : level_(level), enabled_(Logger::enabled(level)) {}
+
+void LogStream::commit() {
+    done_ = true;
+    if (!enabled_) return;
+    std::ostream& sink = (level_ >= LogLevel::Warn) ? std::cerr : std::cout;
+    std::lock_guard<std::mutex> lock(g_mutex);
+    sink << timestamp() << " [" << tag(level_) << "] " << buffer_.str() << '\n';
+    sink.flush();
+}
+
+}  // namespace fgc

src/core/TelemetryParser.cpp

@@ -0,0 +1,55 @@
+#include "fgc/TelemetryParser.h"
+
+#include <vector>
+
+namespace fgc {
+
+namespace {
+
+std::vector<std::string> split(const std::string& s, char delim) {
+    std::vector<std::string> out;
+    std::string cur;
+    for (char c : s) {
+        if (c == delim) {
+            out.push_back(cur);
+            cur.clear();
+        } else if (c != '\r' && c != '\n') {
+            cur += c;
+        }
+    }
+    out.push_back(cur);
+    return out;
+}
+
+}  // namespace
+
+std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
+    if (line.empty() || line[0] != '$') return std::nullopt;
+
+    std::vector<std::string> f = split(line, ';');
+    // Need indices 0..11: '$' marker + 11 data fields.
+    if (f.size() < 12) return std::nullopt;
+
+    try {
+        MotorTelemetry t;
+        t.encoder        = std::stoi(f[1]);
+        t.encoder_err    = std::stoi(f[2]);
+        t.sgt_val        = std::stoi(f[3]);
+        t.sgt_stat       = std::stoi(f[4]);
+        t.is_moving      = std::stoi(f[5]);
+        t.control_status = std::stoi(f[6]);
+        t.heading        = std::stof(f[7]);
+        t.deviation_warn = std::stoi(f[8]);
+        // NOTE: field order is humidity (index 9) BEFORE temperature (index 10),
+        // matching the controller firmware. See docs/known-issues.md #6 - do not
+        // swap without confirming the firmware output.
+        t.humidity       = std::stoi(f[9]);
+        t.temperature    = std::stoi(f[10]);
+        t.fan_pwm        = std::stoi(f[11]);
+        return t;
+    } catch (const std::exception&) {
+        return std::nullopt;
+    }
+}
+
+}  // namespace fgc