From 14707c62ae694c6fb148a0a50b385f4881b7bcec Mon Sep 17 00:00:00 2001 From: pgdalmeida Date: Mon, 22 Jun 2026 23:44:13 +0200 Subject: [PATCH] Migrate host to the current firmware serial protocol (counts/MOVE/ST) --- CMakeLists.txt | 2 + README.md | 10 +-- config/config.example.ini | 29 +++++++ config/scan.csv | 24 ++++++ docs/configuration.md | 46 ++++++++-- docs/known-issues.md | 6 +- docs/mqtt-api.md | 15 ++-- include/fgc/CaptureScheduler.h | 37 +++++--- include/fgc/Config.h | 15 ++++ include/fgc/Geometry.h | 33 ++++++++ include/fgc/IControlChannel.h | 5 +- include/fgc/IMotorController.h | 43 +++++++--- include/fgc/ImagePipeline.h | 16 ++-- include/fgc/ScanGrid.h | 51 +++++++++++ include/fgc/TelemetryParser.h | 15 ++-- include/fgc/mock/MockMotorController.h | 81 ++++++++++++++---- src/camera/ImagePipeline.cpp | 14 +-- src/core/Application.cpp | 69 +++++++++++---- src/core/CaptureScheduler.cpp | 84 ++++++++++++------ src/core/Config.cpp | 40 +++++++++ src/core/Geometry.cpp | 18 ++++ src/core/ScanGrid.cpp | 113 +++++++++++++++++++++++++ src/core/TelemetryParser.cpp | 87 ++++++++++++++----- src/mqtt/MqttControlChannel.cpp | 1 + src/serial/SerialMotorController.cpp | 33 ++++++-- tests/CMakeLists.txt | 2 + tests/test_geometry.cpp | 32 +++++++ tests/test_scangrid.cpp | 53 ++++++++++++ tests/test_scheduler.cpp | 75 ++++++++++++---- tests/test_telemetry.cpp | 53 ++++++++---- 30 files changed, 917 insertions(+), 185 deletions(-) create mode 100644 config/scan.csv create mode 100644 include/fgc/Geometry.h create mode 100644 include/fgc/ScanGrid.h create mode 100644 src/core/Geometry.cpp create mode 100644 src/core/ScanGrid.cpp create mode 100644 tests/test_geometry.cpp create mode 100644 tests/test_scangrid.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a11bc9..714ada0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,8 @@ add_library(fgc_core STATIC src/core/Config.cpp src/core/Paths.cpp src/core/Logger.cpp + src/core/Geometry.cpp + src/core/ScanGrid.cpp src/core/TelemetryParser.cpp src/core/CaptureScheduler.cpp src/core/CommandParser.cpp diff --git a/README.md b/README.md index e37e207..480e895 100644 --- a/README.md +++ b/README.md @@ -125,12 +125,12 @@ Lines are tagged by category with a `TX`/`RX` direction, so a single subsystem i or `grep`: ``` -14:02:01.234 [SERIAL] TX kd180 -14:02:01.250 [SERIAL] RX $;1234;0;500;1;1;0;180.5;0;45;27;128; +14:02:01.234 [SERIAL] TX MOVE -90,0 +14:02:01.250 [SERIAL] RX ST Y:A,982,969,80084000,0,8,8,Se P:A,... 14:02:02.000 [MQTT] PUB GGS/FWT/Tower/StatusCode 0 14:02:02.500 [CAMERA] TX trigger cam0 14:02:02.910 [CAMERA] RX frame cam0 1936x1216 7064576B -14:02:02.000 [CONTROL] motor cmd "p" (auto sweep) +14:02:02.000 [CONTROL] sweep -> grid yaw=-90 pitch=0 ``` Example — watch only firmware serial traffic on the device: @@ -181,11 +181,11 @@ libjxl-dev`, plus the Vimba X SDK under `/opt/VimbaX` for `-DWITH_VIMBA=ON`. ``` CMakeLists.txt Build (options: WITH_VIMBA, WITH_MQTT, BUILD_TESTING) cmake/ FindVmb.cmake (Vimba X), Paho.cmake (FetchContent) -config/ config.example.ini (real config.ini is gitignored) +config/ config.example.ini, scan.csv (real config.ini is gitignored) include/fgc/ Public headers: interfaces, Config, Logger, scheduler, impls mock/ Mock/null implementations src/ - core/ Config, Logger, Paths, parsers, CaptureScheduler, Application + core/ Config, Logger, Paths, parsers, Geometry, ScanGrid, CaptureScheduler, Application serial/ SerialMotorController mqtt/ MqttControlChannel camera/ VimbaCameraSource, ImagePipeline, JpegXlEncoder diff --git a/config/config.example.ini b/config/config.example.ini index 6600426..8ab366d 100644 --- a/config/config.example.ini +++ b/config/config.example.ini @@ -35,6 +35,35 @@ id_Cam4 = device = /dev/ttyACM0 baud = 115200 +[Motor] +; Degrees<->encoder-counts calibration for each axis. The firmware speaks only +; in absolute encoder counts; these map them to the heading/elevation degrees +; used by MQTT (target_HDG) and CamEvent. CALIBRATE on the rig after homing: +; counts = zero_count + deg * counts_per_deg +; counts_per_deg may be negative to flip direction. min/max_deg clamp commands. +; Yaw starting point: firmware steps_per_rev=177000 over ~180 deg => ~983.33. +yaw_counts_per_deg = 983.33 +yaw_zero_count = 500000 +yaw_min_deg = -90 +yaw_max_deg = 90 +; Pitch (physical endstops, ~0..60 deg). CALIBRATE counts_per_deg/zero_count. +pitch_counts_per_deg = 8333.33 +pitch_zero_count = 0 +pitch_min_deg = 0 +pitch_max_deg = 60 + +[Scan] +; Capture scan grid (the (yaw,pitch) waypoints auto-sweep steps through). +; If grid_file is set, that CSV is used verbatim (one "yaw_deg,pitch_deg" per +; line, '#' comments allowed) - edit it to define exact scan coordinates. +; If grid_file is blank, a grid is generated: yaw_intervals positions across +; [yaw_min_deg, yaw_max_deg] at each pitch level, traversed ping-pong. +grid_file = +yaw_intervals = 56 +yaw_min_deg = -90 +yaw_max_deg = 90 +pitch_levels = 10,30,50 + [Paths] ; Directory for saved .jxl images. Supports leading ~ and $ENV expansion. ; If blank, defaults to $XDG_DATA_HOME/fire_gimbal_control/images diff --git a/config/scan.csv b/config/scan.csv new file mode 100644 index 0000000..14a7873 --- /dev/null +++ b/config/scan.csv @@ -0,0 +1,24 @@ +# Scan grid — capture waypoints, one "yaw_deg,pitch_deg" per line. +# Edit these to define the exact scan coordinates. Blank lines and lines +# starting with '#' are ignored. The auto-sweep (ControlCode 0) walks the list +# forward then backward (ping-pong), triggering the cameras at each settled +# point. Degrees are converted to encoder counts via the [Motor] calibration. +# +# Reference this file from config.ini: [Scan] grid_file = config/scan.csv +# +# Example: three elevation rows across a 180° yaw arc. +-90,10 +-45,10 +0,10 +45,10 +90,10 +-90,30 +-45,30 +0,30 +45,30 +90,30 +-90,50 +-45,50 +0,50 +45,50 +90,50 diff --git a/docs/configuration.md b/docs/configuration.md index 7a97bc0..bb4d1ba 100644 --- a/docs/configuration.md +++ b/docs/configuration.md @@ -42,6 +42,23 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config | `Features` | `mock_serial` | bool | `false` | Use the simulated motor controller | | `Logging` | `level` | enum | `info` | Linear log level (`--log-level` overrides) | | `Logging` | `trace` | csv | — | Wire-trace categories, off by default (`--trace` overrides) | +| `Motor` | `yaw_counts_per_deg` / `pitch_counts_per_deg` | float | `983.33` / — | Encoder counts per degree (**calibrate**; may be negative to flip) | +| `Motor` | `yaw_zero_count` / `pitch_zero_count` | int | `500000` / `0` | `xenc` value that = 0° | +| `Motor` | `yaw_min_deg`/`yaw_max_deg`/`pitch_*` | float | `-90`/`90`/… | Soft clamp on commanded degrees | +| `Scan` | `grid_file` | string | — | CSV of `yaw_deg,pitch_deg` waypoints; empty → generate | +| `Scan` | `yaw_intervals` | int | `56` | Generated yaw positions across `[yaw_min_deg, yaw_max_deg]` | +| `Scan` | `yaw_min_deg`/`yaw_max_deg` | float | `-90`/`90` | Generated yaw arc | +| `Scan` | `pitch_levels` | csv | `0` | Generated pitch elevations (deg) | + +### `[Motor]` calibration & `[Scan]` grid + +The firmware reports only **encoder counts**; `[Motor]` maps them to the heading/elevation degrees +used by MQTT (`target_HDG`) and `CamEvent`. Calibrate `*_counts_per_deg` / `*_zero_count` against +real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`). + +The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits (ping-pong). Set +`[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact +coordinates, or leave it blank to generate `yaw_intervals × pitch_levels` points. Camera index → output subfolder defaults to `RGB`, `ACR`, `NIR` (`CameraConfig::labels`). @@ -72,7 +89,8 @@ Typical headless dev run: `scripts/run.sh --mock-serial --mock-camera --no-mqtt ### Init sequence (`--init`) -Sends to the motor controller, with delays: `r` → `e` → `q` → *(wait 60 s)* → `y` → `ud56` +Sends `ENABLE Y`, `ENABLE P`, `HOME`, then polls telemetry until both axes report `READY` +(firmware `ST` state `A`), bounded by the firmware's 60 s homing timeout, then sets `SPEED Y/P` ([src/core/Application.cpp](../src/core/Application.cpp), `runInitSequence`). ## Interactive console commands @@ -93,7 +111,7 @@ Handled in `Application::Impl::handleCommand`. | `set camera display <0\|1>` | Toggle OpenCV preview window | | `set camera fps ` | Camera acquisition frame rate (real camera only) | | `set fps ` | Capture interval rate (images/second) | -| `set motorctl ` | Forward a raw command to the motor controller (e.g. `set motorctl kd180`) | +| `set motorctl ` | Forward a raw command to the motor controller (e.g. `set motorctl MOVE Y 20000`) | | `exit` | Quit (Ctrl-D also works) | ## Logging: level vs. wire-trace categories @@ -114,19 +132,29 @@ it does not merge). At runtime the `trace` console command edits categories incr Trace lines carry a category tag and a `TX`/`RX` direction so one subsystem is easy to follow/grep: ``` -[SERIAL] TX kd180 (command to firmware) -[SERIAL] RX $;1234;0;500;1;1;0;180.5;... (telemetry from firmware; RX(unparsed) on a bad line) +[SERIAL] TX MOVE -90,0 (command to firmware: yaw,pitch counts) +[SERIAL] RX ST Y:A,982,969,80084000,0,8,8,Se P:A,... (status from firmware) [MQTT] PUB GGS/FWT/Tower/StatusCode 0 / [MQTT] RX GGS/FWT/Tower/target_HDG 180 [CAMERA] TX trigger cam0 / [CAMERA] RX frame cam0 1936x1216 7064576B -[CONTROL] motor cmd "p" (auto sweep) (scheduler decisions + inbound console commands) +[CONTROL] sweep -> grid yaw=-90 pitch=0 (scheduler decisions + inbound console commands) ``` ## Motor command vocabulary (emitted by the software) +The firmware speaks **full-word, newline-terminated** commands in absolute **encoder counts** +(see `../firmware/docs/protocol.md`). The host converts degrees↔counts via the `[Motor]` calibration. + | Command | When | Meaning | |---------|------|---------| -| `r`,`e`,`q`,`y`,`ud56` | init sequence | homing / set intervals-per-turn | -| `p` | ControlCode 0 capture | stop / advance to next interval | -| `kd` | ControlCode 1 capture | drive to target heading | +| `ENABLE Y` / `ENABLE P` | init | energize the axis coils | +| `HOME` | init | home all axes (firmware runs it non-blocking; watch `ST` state `R→H→A`) | +| `SPEED Y\|P ` | init | set the production move speed (VMAX, counts/s) | +| `MOVE ,` | each capture point | drive both axes to absolute counts (combined form) | +| `MOVE Y\|P ` | — | single-axis absolute move (`set motorctl`) | +| `STOP Y\|P\|ALL` | — | ramp to a controlled stop | -These are interpreted by the motor-controller firmware (not in this repo). +Capture is a **move → settle → trigger** cycle: the scheduler issues a `MOVE`, waits until both +axes report standstill at the target, then triggers the cameras. **ControlCode 0** walks the scan +grid (ping-pong); **ControlCode 1** drives yaw to `target_HDG` (pitch held). Telemetry arrives as +firmware `ST` lines (per-axis state + encoder counts), parsed by +[TelemetryParser](../src/core/TelemetryParser.cpp). diff --git a/docs/known-issues.md b/docs/known-issues.md index f5e333d..6c6e157 100644 --- a/docs/known-issues.md +++ b/docs/known-issues.md @@ -17,6 +17,8 @@ refactor did about them. | 10 | `parser()` missing return | Old parser removed; `parseTelemetryLine` returns `std::optional` cleanly | | 11 | Fragile Boost.Spirit command grammar | Replaced by `parseCommand` whitespace tokenizer ([src/core/CommandParser.cpp](../src/core/CommandParser.cpp)) | | 12 | Two divergent `config.ini` files | Single committed `config/config.example.ini`; real configs gitignored | +| 6 | Telemetry field order: humidity before temperature | **Obsolete** — migrated to the current firmware's `ST` protocol (encoder counts, no environmental fields); humidity/temp/fan are gone | +| 7 | Trigger fires while `is_moving == 1` (not when stopped) | **Fixed** by the protocol migration — capture is now move → **settle** → trigger; the camera fires only once both axes report standstill at the target ([CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp)) | Also added along the way: a leveled logger, typed/validated config, an SDK-independent core library, and a doctest unit-test suite (`ctest`). @@ -25,8 +27,8 @@ doctest unit-test suite (`ctest`). | # | Issue | Status | |---|-------|--------| -| 6 | Telemetry field order: humidity (field 9) before temperature (field 10) | **Preserved as-is** with a clear comment in [TelemetryParser.cpp](../src/core/TelemetryParser.cpp). Confirm against the motor-controller firmware before changing. | -| 7 | Trigger fires while `is_moving == 1` (rather than when stopped) | **Preserved** behind a named predicate in [CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp). Looks like a bug; confirm the firmware's `is_moving` semantics, then flip if needed. | +| 13 | `[Motor]` degrees↔counts calibration | The `config.example.ini` values are **placeholders**. Calibrate `*_counts_per_deg` / `*_zero_count` against real `xenc` readings after homing on the rig. | +| 14 | Settle tolerance / timing | `kSettleTolCounts` (600) and the per-interval timing in [CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) are untested against hardware; tune against observed `ST` behaviour. | ## Verification caveats diff --git a/docs/mqtt-api.md b/docs/mqtt-api.md index 8bb07b3..fcb9425 100644 --- a/docs/mqtt-api.md +++ b/docs/mqtt-api.md @@ -29,7 +29,7 @@ Subscribed on (re)connect; handled in `MqttControlChannel::message_arrived()` | Topic | Payload | Parsed as | Effect | |-------|---------|-----------|--------| -| `GGS/FWT//target_HDG` | integer heading as string, e.g. `"137"` | validated with `stoi`, stored as **string** | Sets the target heading used in ControlCode 1 (`kd`) | +| `GGS/FWT//target_HDG` | integer heading as string, e.g. `"137"` | validated with `stoi`, stored as **string** | Sets the yaw target used in ControlCode 1 (converted to encoder counts via `[Motor]`) | | `GGS/FWT//ControlCode` | integer as string, `"0"` or `"1"` | `stoi` → int | Selects the capture mode (see below) | Invalid (non-integer) payloads are caught and logged; the previous value is kept. @@ -39,10 +39,10 @@ flags** so each update is acted on once (`MqttControlChannel::poll()`). ### ControlCode semantics -| ControlCode | Behaviour ([main.cpp](../main.cpp) lines 293-334) | +| ControlCode | Behaviour ([CaptureScheduler](../src/core/CaptureScheduler.cpp)) | |-------------|---------------------------------------------------| -| `0` | **Automatic sweep.** On each interval, send `p` to advance/stop the gimbal, then trigger cameras. | -| `1` | **Directed.** On each interval, send `kd` to drive to the heading from `target_HDG`, then trigger. | +| `0` | **Automatic sweep.** Walk the scan grid (ping-pong): `MOVE` to the next `(yaw,pitch)` waypoint, wait for standstill, trigger cameras. | +| `1` | **Directed.** `MOVE` yaw to `target_HDG` (pitch held), wait for standstill, trigger. | When a ControlCode message arrives, the program echoes the current code back on the StatusCode topic. @@ -58,14 +58,15 @@ When a ControlCode message arrives, the program echoes the current code back on Built by `MqttControlChannel::publishCamEvent` from a `CamEvent`: ```json -{ "fwt":"Staeffelsberg", "cam":"RGB", "hdg":1373, "time":1719312345678 } +{ "fwt":"Staeffelsberg", "cam":"RGB", "hdg":1373, "pit":300, "time":1719312345678 } ``` | Field | Type | Meaning | |-------|------|---------| | `fwt` | string | Tower name (`config.ini` `tower_name`) | | `cam` | string | Camera label: `RGB`, `ACR`, or `NIR` (by camera index) | -| `hdg` | int | Gimbal heading **× 10** (one decimal place encoded as integer) at capture time | +| `hdg` | int | Gimbal yaw heading **× 10** (one decimal place encoded as integer) at capture time | +| `pit` | int | Gimbal pitch elevation **× 10** at capture time (derived from the pitch encoder) | | `time` | int | Capture timestamp, Unix epoch **milliseconds** (matches the `.jxl` filename) | > The `time` value is the same Unix-ms timestamp used as the image filename, so a consumer can locate the file @@ -78,7 +79,7 @@ subscribe: GGS/FWT//target_HDG (int heading) GGS/FWT//ControlCode (0 = auto sweep, 1 = directed) publish: GGS/FWT//StatusCode (echoed control code) - GGS/FWT//CamEvent (JSON: fwt, cam, hdg×10, time-ms) + GGS/FWT//CamEvent (JSON: fwt, cam, hdg×10, pit×10, time-ms) ``` ## Local testing diff --git a/include/fgc/CaptureScheduler.h b/include/fgc/CaptureScheduler.h index 57b7874..af30fce 100644 --- a/include/fgc/CaptureScheduler.h +++ b/include/fgc/CaptureScheduler.h @@ -1,35 +1,40 @@ #pragma once +#include "fgc/Geometry.h" #include "fgc/ICameraSource.h" #include "fgc/IControlChannel.h" #include "fgc/IMotorController.h" +#include "fgc/ScanGrid.h" #include #include 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. +// The capture control loop, 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. +// 2. reads motor telemetry (per-axis encoder counts + state), +// 3. runs the capture cycle as a move -> settle -> trigger machine: when the +// interval elapses it issues an absolute `MOVE ,` to the next +// target, waits until both axes report standstill at that target, then +// software-triggers the cameras. // -// ControlCode 0 = automatic sweep ("p"); ControlCode 1 = drive to the -// MQTT-supplied target heading ("kd"). +// ControlCode 0 = automatic sweep through the ScanGrid (ping-pong); +// ControlCode 1 = drive yaw to the MQTT-supplied target heading (pitch held). +// Degrees are converted to encoder counts via Geometry. 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, + CaptureScheduler(IMotorController& motor, ICameraSource& camera, IControlChannel& channel, + double image_rate, Geometry geometry, ScanGrid& grid, std::function now_ms = {}); - void setCaptureActive(bool active); // mirrors the old cam_started flag + void setCaptureActive(bool active); bool captureActive() const { return capture_active_; } void setImageRate(double rate); // images per second @@ -41,14 +46,21 @@ public: // Inspection (mainly for tests). int controlCode() const { return control_code_; } std::string targetHeading() const { return target_heading_; } + long yawTargetCounts() const { return yaw_target_; } + long pitchTargetCounts() const { return pitch_target_; } + bool moving() const { return moving_; } private: long long elapsedMs() const; void resetTimer(); + void sendMove(); // emit MOVE to (yaw_target_, pitch_target_) + bool settledAt(const MotorTelemetry& t) const; IMotorController& motor_; ICameraSource& camera_; IControlChannel& channel_; + Geometry geometry_; + ScanGrid& grid_; std::function now_ms_; double image_rate_; @@ -57,7 +69,10 @@ private: bool capture_active_ = false; int control_code_ = 0; std::string target_heading_ = "0"; - bool trigger_after_stopping_ = false; + + bool moving_ = false; // a MOVE has been issued, awaiting settle + long yaw_target_ = 0; // current target, encoder counts + long pitch_target_ = 0; }; } // namespace fgc diff --git a/include/fgc/Config.h b/include/fgc/Config.h index f5c6f90..9f48dd6 100644 --- a/include/fgc/Config.h +++ b/include/fgc/Config.h @@ -1,5 +1,7 @@ #pragma once +#include "fgc/Geometry.h" + #include #include #include @@ -50,6 +52,17 @@ struct LoggingConfig { std::string trace; // verbatim wire-trace categories: serial,mqtt,camera,control,all,none }; +// [Scan]: source of the capture scan grid (the (yaw,pitch) waypoints the +// auto-sweep steps through). If grid_file is set, the CSV is loaded verbatim; +// otherwise a grid is generated from the parameters below (see ScanGrid). +struct ScanConfig { + std::string grid_file; // path to an editable yaw_deg,pitch_deg CSV; empty => generate + int yaw_intervals = 56; // generated: yaw positions across [yaw_min_deg, yaw_max_deg] + double yaw_min_deg = -90.0; + double yaw_max_deg = 90.0; + std::string pitch_levels = "0"; // generated: comma list of pitch elevations (deg) +}; + struct AppConfig { GeneralConfig general; NetworkConfig network; @@ -58,6 +71,8 @@ struct AppConfig { PathsConfig paths; FeaturesConfig features; LoggingConfig logging; + Geometry geometry; // [Motor] degrees<->counts maps (yaw + pitch) + ScanConfig scan; // [Scan] grid source // Capture rate in images/second (derived from general.image_interval). double image_rate() const; diff --git a/include/fgc/Geometry.h b/include/fgc/Geometry.h new file mode 100644 index 0000000..ce569cd --- /dev/null +++ b/include/fgc/Geometry.h @@ -0,0 +1,33 @@ +#pragma once + +namespace fgc { + +// Affine map between heading degrees and absolute encoder counts for one axis: +// +// counts = zero_count + deg * counts_per_deg +// deg = (counts - zero_count) / counts_per_deg +// +// The firmware speaks only in encoder counts; this is how the host converts to +// the heading/elevation degrees used by the MQTT contract and CamEvent. The +// parameters are operator-calibrated against real `xenc` readings after homing +// (counts_per_deg may be negative to flip the direction sense). +struct AxisMap { + double counts_per_deg = 1.0; // calibrate on the rig + long zero_count = 0; // xenc value that corresponds to 0 deg + double min_deg = -100000.0; // soft clamp applied by toCounts (degrees) + double max_deg = 100000.0; + + // Degrees -> counts. Clamps `deg` to [min_deg, max_deg] and rounds. + long toCounts(double deg) const; + + // Counts -> degrees. Returns 0 if counts_per_deg is ~0 (misconfigured). + double toDeg(long counts) const; +}; + +// Per-axis maps for the two-axis gimbal. +struct Geometry { + AxisMap yaw; + AxisMap pitch; +}; + +} // namespace fgc diff --git a/include/fgc/IControlChannel.h b/include/fgc/IControlChannel.h index a370d3e..090a275 100644 --- a/include/fgc/IControlChannel.h +++ b/include/fgc/IControlChannel.h @@ -10,14 +10,15 @@ 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" + std::string target_heading; // kept as string; the yaw target in degrees }; // 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) + int heading_decideg = 0; // yaw heading * 10 (one decimal as integer) + int pitch_decideg = 0; // pitch elevation * 10 (one decimal as integer) long long timestamp_ms = 0; // Unix epoch ms; matches the image filename }; diff --git a/include/fgc/IMotorController.h b/include/fgc/IMotorController.h index dd6bf29..8ee5148 100644 --- a/include/fgc/IMotorController.h +++ b/include/fgc/IMotorController.h @@ -4,19 +4,35 @@ namespace fgc { -// Telemetry snapshot from the motor controller (was struct motor_info). +// Per-axis lifecycle state, from the firmware ST line's state char (B/R/H/A/E). +enum class AxisState { Boot, Reset, Homing, Ready, Error, Unknown }; + +// One axis segment of a firmware `ST` status line: +// ST Y:,,,,,,, [P:...] +// Positions are absolute encoder counts; degrees are derived via Geometry. +struct AxisTelemetry { + AxisState state = AxisState::Unknown; + long xactual = 0; // TMC step counter (commanded position) + long xenc = 0; // encoder position (actual shaft position) + unsigned drv_status = 0; // DRV_STATUS register (decoded from the 8-digit hex) + int sg = 0; // SG_RESULT + int cs = 0; // CS_ACTUAL + int pwm = 0; // PWM_SCALE_SUM + bool standstill = false; // flag 'S' (DRV_STST) + bool stall = false; // flag 's' + bool overtemp = false; // flag 'o'/'O' + bool endstop_l = false; // flag 'L' + bool endstop_r = false; // flag 'R' + + bool moving() const { return state == AxisState::Homing || !standstill; } + bool ready() const { return state == AxisState::Ready; } +}; + +// Telemetry snapshot from the motor controller: one segment per axis. 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 + AxisTelemetry yaw; + AxisTelemetry pitch; + bool pitch_present = false; // was a P: segment seen? }; // Abstraction over the gimbal's motor controller. Implemented by @@ -30,7 +46,8 @@ public: virtual void start() = 0; virtual void stop() = 0; - // Send a raw command string to the controller (e.g. "p", "kd180"). + // Send a command line to the controller (e.g. "HOME", "MOVE Y 20000"). + // Implementations append the newline terminator the firmware requires. virtual void sendCommand(const std::string& cmd) = 0; // Latest telemetry snapshot (thread-safe in implementations). diff --git a/include/fgc/ImagePipeline.h b/include/fgc/ImagePipeline.h index 62439fe..108cdb9 100644 --- a/include/fgc/ImagePipeline.h +++ b/include/fgc/ImagePipeline.h @@ -14,6 +14,12 @@ namespace fgc { +// Gimbal orientation in degrees, stamped onto each CamEvent. +struct Orientation { + float yaw_deg = 0.f; + float pitch_deg = 0.f; +}; + // Consumes captured frames and turns them into stored artifacts: rotate 90 deg // CCW, encode to JPEG XL, write to /