Migrate host to the current firmware serial protocol (counts/MOVE/ST)

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

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.234 [SERIAL]  TX MOVE -90,0
-14:02:01.250 [SERIAL]  RX $;1234;0;500;1;1;0;180.5;0;45;27;128;
+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

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

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

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 <v>` | Camera acquisition frame rate (real camera only) |
 | `set fps <v>` | Capture interval rate (images/second) |
-| `set motorctl <cmd>` | Forward a raw command to the motor controller (e.g. `set motorctl kd180`) |
+| `set motorctl <cmd>` | 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]  TX MOVE -90,0                     (command to firmware: yaw,pitch counts)
-[SERIAL]  RX $;1234;0;500;1;1;0;180.5;...  (telemetry from firmware; RX(unparsed) on a bad line)
+[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 |
+| `ENABLE Y` / `ENABLE P` | init | energize the axis coils |
-| `p` | ControlCode 0 capture | stop / advance to next interval |
+| `HOME` | init | home all axes (firmware runs it non-blocking; watch `ST` state `R→H→A`) |
-| `kd<heading>` | ControlCode 1 capture | drive to target heading |
+| `SPEED Y\|P <vel>` | init | set the production move speed (VMAX, counts/s) |
+| `MOVE <yaw>,<pitch>` | each capture point | drive both axes to absolute counts (combined form) |
+| `MOVE Y\|P <pos>` | — | 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).

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. |
+| 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. |
-| 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. |
+| 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
 

docs/mqtt-api.md

@@ -29,7 +29,7 @@ Subscribed on (re)connect; handled in `MqttControlChannel::message_arrived()`
 
 | Topic | Payload | Parsed as | Effect |
 |-------|---------|-----------|--------|
-| `GGS/FWT/<tower>/target_HDG` | integer heading as string, e.g. `"137"` | validated with `stoi`, stored as **string** | Sets the target heading used in ControlCode 1 (`kd<heading>`) |
+| `GGS/FWT/<tower>/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/<tower>/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. |
+| `0` | **Automatic sweep.** Walk the scan grid (ping-pong): `MOVE` to the next `(yaw,pitch)` waypoint, wait for standstill, trigger cameras. |
-| `1` | **Directed.** On each interval, send `kd<target_HDG>` to drive to the heading from `target_HDG`, then trigger. |
+| `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/<tower>/target_HDG      (int heading)
             GGS/FWT/<tower>/ControlCode     (0 = auto sweep, 1 = directed)
 
 publish:    GGS/FWT/<tower>/StatusCode      (echoed control code)
-            GGS/FWT/<tower>/CamEvent        (JSON: fwt, cam, hdg×10, time-ms)
+            GGS/FWT/<tower>/CamEvent        (JSON: fwt, cam, hdg×10, pit×10, time-ms)
 ```
 
 ## Local testing

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 <functional>
 #include <string>
 
 namespace fgc {
 
-// The capture control loop, extracted from the old main() while-loop and
+// The capture control loop, expressed against the I/O interfaces so it can be
-// expressed against the I/O interfaces so it can be unit-tested with mocks.
+// 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,
+//   2. reads motor telemetry (per-axis encoder counts + state),
-//   3. runs the capture cycle: when the configured interval elapses, stops /
+//   3. runs the capture cycle as a move -> settle -> trigger machine: when the
-//      points the gimbal, then software-triggers the cameras.
+//      interval elapses it issues an absolute `MOVE <yaw>,<pitch>` 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
+// ControlCode 0 = automatic sweep through the ScanGrid (ping-pong);
-// MQTT-supplied target heading ("kd<heading>").
+// 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,
+    CaptureScheduler(IMotorController& motor, ICameraSource& camera, IControlChannel& channel,
-                     IControlChannel& channel, double image_rate,
+                     double image_rate, Geometry geometry, ScanGrid& grid,
                      std::function<long long()> 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<long long()> 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

include/fgc/Config.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "fgc/Geometry.h"
+
 #include <map>
 #include <string>
 #include <vector>
@@ -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;

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

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<heading>"
+    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
 };
 

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:<state>,<xactual>,<xenc>,<drv_hex>,<sg>,<cs>,<pwm>,<flags> [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
+    AxisTelemetry yaw;
-    int   encoder_err    = 0;   // Xerr
+    AxisTelemetry pitch;
-    int   sgt_val        = 0;   // StallGuard value
+    bool          pitch_present = false;  // was a P: segment seen?
-    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
@@ -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).

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 <output_dir>/<label>/<timestamp>.jxl, and
 // publish a CamEvent. Runs a background worker so encoding never blocks
@@ -32,8 +38,8 @@ public:
         bool                     display = false;        // show an OpenCV preview window
     };
 
-    // heading: returns the current gimbal heading (degrees) for CamEvent.
+    // orientation: returns the current gimbal yaw+pitch (degrees) for CamEvent.
-    ImagePipeline(IControlChannel& channel, std::function<float()> heading, Params params);
+    ImagePipeline(IControlChannel& channel, std::function<Orientation()> orientation, Params params);
     ~ImagePipeline();
 
     void start();
@@ -53,7 +59,7 @@ private:
     std::string labelFor(int cam_id) const;
 
     IControlChannel&             channel_;
-    std::function<float()>  heading_;
+    std::function<Orientation()> orientation_;
     Params                       params_;
 
     std::queue<Frame>       queue_;

include/fgc/ScanGrid.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "fgc/Config.h"
+
+#include <cstddef>
+#include <vector>
+
+namespace fgc {
+
+// One capture waypoint, in degrees (converted to encoder counts at command time
+// via Geometry).
+struct ScanPoint {
+    double yaw_deg   = 0.0;
+    double pitch_deg = 0.0;
+};
+
+// The ordered list of scan waypoints plus a ping-pong (boustrophedon) cursor:
+// the auto-sweep walks the list forward to the last point, then backward to the
+// first, repeating - so a bounded (e.g. 180 deg) yaw arc never has to wrap.
+//
+// Points come either from an operator-edited CSV (ScanConfig::grid_file) or, if
+// none is given, are generated from the [Scan] parameters.
+class ScanGrid {
+public:
+    ScanGrid() = default;
+    explicit ScanGrid(std::vector<ScanPoint> points) : points_(std::move(points)) {}
+
+    // Build from config: load grid_file if set (throws std::runtime_error on a
+    // missing/!malformed file), else generate from the parameters.
+    static ScanGrid fromConfig(const ScanConfig& cfg);
+
+    bool             empty() const { return points_.empty(); }
+    std::size_t      size() const { return points_.size(); }
+    const ScanPoint& current() const { return points_[index_]; }
+    const std::vector<ScanPoint>& points() const { return points_; }
+
+    // Step the cursor one waypoint, reversing direction at either end.
+    void advance();
+    void reset() { index_ = 0; forward_ = true; }
+
+private:
+    std::vector<ScanPoint> points_;
+    std::size_t            index_   = 0;
+    bool                   forward_ = true;
+};
+
+// Exposed for unit testing.
+std::vector<ScanPoint> parseScanCsv(const std::string& text);
+std::vector<ScanPoint> generateScanGrid(const ScanConfig& cfg);
+
+}  // namespace fgc

include/fgc/TelemetryParser.h

@@ -7,13 +7,18 @@
 
 namespace fgc {
 
-// Parse a motor-controller telemetry line of the form:
+// Parse a firmware `ST` status line:
 //
-//   $;Xenc;Xerr;sgt_val;sgt_stat;is_moving;control_status;hdg;deviation_warn;humid;temp;fan_pwm;
+//   ST Y:<state>,<xactual>,<xenc>,<drv_hex>,<sg>,<cs>,<pwm>,<flags> [P:...]
 //
-// i.e. a leading '$' marker followed by 11 ';'-separated numeric fields
+// where <state> is B/R/H/A/E and <flags> is an optional run of status letters
-// (a trailing ';' is tolerated). Returns nullopt if the line does not start
+// (S=standstill, s=stall, o/O=overtemp, L/R=endstops, e=eeprom). Returns nullopt
-// with '$', has too few fields, or a field fails to convert.
+// for any line that is not a well-formed `ST` line (acks and other async output
+// like OK/ERR/DG/DUMP are handled by the caller). A missing/garbled axis segment
+// yields nullopt rather than partial data.
 std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line);
 
+// Parse a single "Y:..." / "P:..." axis segment. Exposed for unit testing.
+std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg);
+
 }  // namespace fgc

include/fgc/mock/MockMotorController.h

@@ -4,13 +4,16 @@
 #include "fgc/Logger.h"
 
 #include <mutex>
+#include <sstream>
 #include <string>
 
 namespace fgc {
 
-// Simulated motor controller for development without hardware. Pretends the
+// Simulated motor controller for development without hardware. Models the new
-// gimbal is continuously sweeping: heading advances on each telemetry read and
+// firmware's encoder-count protocol: HOME makes both axes READY; MOVE sets a
-// is_moving stays 1 so the capture cycle fires. "kd<heading>" sets the target.
+// per-axis target; each telemetry() read steps the encoder toward its target
+// and reports standstill once reached - so the capture scheduler's
+// move -> settle -> trigger loop runs exactly as it would against real hardware.
 class MockMotorController : public IMotorController {
 public:
     void start() override { LOG_INFO << "[mock] motor controller started"; }
@@ -19,29 +22,75 @@ public:
     void sendCommand(const std::string& cmd) override {
         LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
         std::lock_guard<std::mutex> lock(mutex_);
-        if (cmd.rfind("kd", 0) == 0) {
+        std::istringstream in(cmd);
-            try {
+        std::string verb;
-                tel_.heading = std::stof(cmd.substr(2));
+        in >> verb;
-            } catch (const std::exception&) {}
+        for (auto& c : verb) c = static_cast<char>(std::toupper(static_cast<unsigned char>(c)));
+
+        if (verb == "HOME") {
+            homed_ = true;
+        } else if (verb == "MOVE") {
+            std::string a;
+            in >> a;
+            auto comma = a.find(',');
+            if (comma != std::string::npos) {  // MOVE <yaw>,<pitch>
+                tryParse(a.substr(0, comma), yaw_target_);
+                tryParse(a.substr(comma + 1), pitch_target_);
+            } else {  // MOVE Y|P <pos>
+                long pos = 0;
+                if (in >> pos) {
+                    if (a == "Y" || a == "y") yaw_target_ = pos;
+                    else if (a == "P" || a == "p") pitch_target_ = pos;
                 }
             }
+        } else if (verb == "STOP") {
+            yaw_target_   = yaw_.xenc;
+            pitch_target_ = pitch_.xenc;
+        }
+        // ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
+    }
 
     MotorTelemetry telemetry() override {
         std::lock_guard<std::mutex> lock(mutex_);
-        tel_.heading += 6.0f;  // simulate a sweep step
+        step(yaw_, yaw_target_);
-        if (tel_.heading >= 360.f) tel_.heading -= 360.f;
+        step(pitch_, pitch_target_);
-        tel_.is_moving = 1;
+        MotorTelemetry t;
-        tel_.temperature = 20;
+        t.yaw           = yaw_;
-        tel_.humidity = 50;
+        t.pitch         = pitch_;
-        tel_.fan_pwm = 0;
+        t.pitch_present = true;
-        return tel_;
+        return t;
     }
 
     bool connected() const override { return true; }
 
 private:
+    static constexpr long kStep = 80000;  // counts advanced per telemetry read
+
+    void step(AxisTelemetry& a, long target) {
+        long d = target - a.xenc;
+        if (d >= -kStep && d <= kStep) {
+            a.xenc       = target;
+            a.standstill = true;
+        } else {
+            a.xenc += (d > 0 ? kStep : -kStep);
+            a.standstill = false;
+        }
+        a.xactual = a.xenc;
+        a.state   = homed_ ? AxisState::Ready : AxisState::Reset;
+    }
+
+    static void tryParse(const std::string& s, long& out) {
+        try {
+            out = std::stol(s);
+        } catch (const std::exception&) {}
+    }
+
     std::mutex    mutex_;
-    MotorTelemetry tel_;
+    AxisTelemetry yaw_;
+    AxisTelemetry pitch_;
+    long          yaw_target_   = 0;
+    long          pitch_target_ = 0;
+    bool          homed_        = false;
 };
 
 }  // namespace fgc

src/camera/ImagePipeline.cpp

@@ -11,9 +11,9 @@ namespace fs = std::filesystem;
 
 namespace fgc {
 
-ImagePipeline::ImagePipeline(IControlChannel& channel, std::function<float()> heading,
+ImagePipeline::ImagePipeline(IControlChannel& channel, std::function<Orientation()> orientation,
                              Params params)
-    : channel_(channel), heading_(std::move(heading)), params_(std::move(params)) {}
+    : channel_(channel), orientation_(std::move(orientation)), params_(std::move(params)) {}
 
 ImagePipeline::~ImagePipeline() { stop(); }
 
@@ -93,10 +93,12 @@ void ImagePipeline::process(const Frame& frame) {
         }
     }
 
+    Orientation o = orientation_ ? orientation_() : Orientation{};
     CamEvent ev;
     ev.tower           = params_.tower;
     ev.camera          = label;
-    ev.heading_decideg = static_cast<int>((heading_ ? heading_() : 0.f) * 10);
+    ev.heading_decideg = static_cast<int>(o.yaw_deg * 10);
+    ev.pitch_decideg   = static_cast<int>(o.pitch_deg * 10);
     ev.timestamp_ms    = frame.timestamp_ms;
     channel_.publishCamEvent(ev);
 

src/core/Application.cpp

@@ -7,6 +7,7 @@
 #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"
@@ -61,6 +62,7 @@ struct Application::Impl {
     std::unique_ptr<ICameraSource>    camera;
     std::unique_ptr<ImagePipeline>    pipeline;
     std::unique_ptr<CaptureScheduler> scheduler;
+    ScanGrid                          grid;  // outlives scheduler (holds a reference to it)
 
     std::atomic<bool>       running{true};
     std::mutex              cmd_mutex;
@@ -111,16 +113,42 @@ struct Application::Impl {
 
     void runInitSequence() {
         using namespace std::chrono_literals;
-        LOG_INFO << "Running gimbal init sequence (find endstops)";
+        LOG_INFO << "Running gimbal init sequence (enable + home)";
-        motor->sendCommand("r");
+        motor->sendCommand("ENABLE Y");
-        std::this_thread::sleep_for(500ms);
+        motor->sendCommand("ENABLE P");
-        motor->sendCommand("e");
+        motor->sendCommand("HOME");  // homes all axes; firmware runs it non-blocking
-        std::this_thread::sleep_for(500ms);
+
-        motor->sendCommand("q");
+        auto allReady = [](const MotorTelemetry& t) {
-        std::this_thread::sleep_for(60s);
+            return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
-        motor->sendCommand("y");
+        };
-        std::this_thread::sleep_for(500ms);
+
-        motor->sendCommand("ud56");  // intervals per turn
+        // 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);
+            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);
+            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";
     }
 
@@ -253,16 +281,27 @@ struct Application::Impl {
         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] { return motor->telemetry().heading; }, pp);
+            *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);
 
-        IMotorController* motor_ptr = motor.get();
-        (void)motor_ptr;
         ImagePipeline* pipe_ptr = pipeline.get();
         camera->setFrameCallback([pipe_ptr](const Frame& f) { pipe_ptr->submit(f); });
 
-        scheduler = std::make_unique<CaptureScheduler>(*motor, *camera, *channel,
+        try {
-                                                       cfg.image_rate());
+            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();

src/core/CaptureScheduler.cpp

@@ -3,6 +3,8 @@
 #include "fgc/Logger.h"
 
 #include <chrono>
+#include <cmath>
+#include <cstdlib>
 
 namespace fgc {
 
@@ -13,33 +15,46 @@ long long steadyNowMs() {
     return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
 }
 
-// Whether a software trigger should fire given the current telemetry.
+// How close (encoder counts) an axis must be to its target, in addition to
-//
+// reporting standstill, to count as "settled". Generous vs. the firmware hold
-// NOTE: this preserves the original behaviour of triggering while the gimbal
+// deadband (~200 counts).
-// reports is_moving == 1. That looks counter-intuitive (one might expect to
+constexpr long kSettleTolCounts = 600;
-// 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,
+                                   IControlChannel& channel, double image_rate, Geometry geometry,
-                                   std::function<long long()> now_ms)
+                                   ScanGrid& grid, std::function<long long()> now_ms)
     : motor_(motor),
       camera_(camera),
       channel_(channel),
+      geometry_(geometry),
+      grid_(grid),
       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::setCaptureActive(bool active) {
+    capture_active_ = active;
+    moving_ = false;  // restart the move/settle cycle cleanly
+    resetTimer();
+}
 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::sendMove() {
+    motor_.sendCommand("MOVE " + std::to_string(yaw_target_) + "," + std::to_string(pitch_target_));
+}
+
+bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
+    return t.yaw.standstill && t.pitch.standstill &&
+           std::labs(t.yaw.xenc - yaw_target_) <= kSettleTolCounts &&
+           std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
+}
+
 void CaptureScheduler::tick() {
     // 1. Remote control input.
     ControlCommand cmd = channel_.poll();
@@ -56,31 +71,46 @@ void CaptureScheduler::tick() {
     // 2. Telemetry.
     MotorTelemetry t = motor_.telemetry();
 
-    // 3. Capture cycle. The interval gate is 1000/rate milliseconds.
+    // 3. Capture cycle. Only ControlCode 0 (sweep) and 1 (directed) act.
     if (control_code_ != 0 && control_code_ != 1) return;
+    if (!capture_active_) 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 (!moving_) {
-        if (t.is_moving == 1) {
+        // Issue the next move once the interval has elapsed.
-            resetTimer();
+        if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;
-            trigger_after_stopping_ = true;
+
         if (control_code_ == 0) {
-                LOG_TRACE_CAT(LogCat::Control) << "motor cmd \"p\" (auto sweep)";
+            if (grid_.empty()) return;
-                motor_.sendCommand("p");
+            const ScanPoint& p = grid_.current();
-            } else {  // control_code_ == 1
+            yaw_target_   = geometry_.yaw.toCounts(p.yaw_deg);
-                LOG_TRACE_CAT(LogCat::Control) << "motor cmd \"kd" << target_heading_ << '"';
+            pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
-                motor_.sendCommand("kd" + target_heading_);
+            LOG_TRACE_CAT(LogCat::Control)
+                << "sweep -> grid yaw=" << p.yaw_deg << " pitch=" << p.pitch_deg;
+        } else {  // ControlCode 1: directed yaw, pitch held at current position.
+            try {
+                yaw_target_ = geometry_.yaw.toCounts(std::stod(target_heading_));
+            } catch (const std::exception&) {
+                return;  // bad heading; wait for a valid one
             }
+            pitch_target_ = t.pitch.xenc;
+            LOG_TRACE_CAT(LogCat::Control) << "directed -> heading " << target_heading_;
         }
+        sendMove();
+        moving_ = true;
+        resetTimer();
+        return;
     }
 
-    if (trigger_after_stopping_ && elapsedMs() > 100) {
+    // Moving: trigger once both axes have settled at the target.
-        if (shouldTrigger(t)) {
+    if (settledAt(t)) {
-            LOG_TRACE_CAT(LogCat::Control) << "trigger decision: is_moving=" << t.is_moving;
+        LOG_TRACE_CAT(LogCat::Control)
+            << "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc << "; triggering";
         if (camera_.trigger()) {
-                trigger_after_stopping_ = false;
+            moving_ = false;
-            }
+            if (control_code_ == 0) grid_.advance();
+            resetTimer();
         }
     }
 }

src/core/Config.cpp

@@ -29,6 +29,29 @@ int getInt(const std::map<std::string, std::string>& kv, const std::string& key,
     }
 }
 
+double getDouble(const std::map<std::string, std::string>& kv, const std::string& key,
+                 double fallback) {
+    auto it = kv.find(key);
+    if (it == kv.end() || it->second.empty()) return fallback;
+    try {
+        return std::stod(it->second);
+    } catch (const std::exception&) {
+        throw std::runtime_error("Config key '" + key + "' must be a number, got '" +
+                                 it->second + "'");
+    }
+}
+
+long getLong(const std::map<std::string, std::string>& kv, const std::string& key, long fallback) {
+    auto it = kv.find(key);
+    if (it == kv.end() || it->second.empty()) return fallback;
+    try {
+        return std::stol(it->second);
+    } catch (const std::exception&) {
+        throw std::runtime_error("Config key '" + key + "' must be an integer, got '" +
+                                 it->second + "'");
+    }
+}
+
 bool getBool(const std::map<std::string, std::string>& kv, const std::string& key, bool fallback) {
     auto it = kv.find(key);
     if (it == kv.end() || it->second.empty()) return fallback;
@@ -89,6 +112,23 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
     cfg.logging.level = get(kv, "Logging.level", cfg.logging.level);
     cfg.logging.trace = get(kv, "Logging.trace", cfg.logging.trace);
 
+    // [Motor]: operator-calibrated degrees<->counts maps (see Geometry).
+    cfg.geometry.yaw.counts_per_deg   = getDouble(kv, "Motor.yaw_counts_per_deg", cfg.geometry.yaw.counts_per_deg);
+    cfg.geometry.yaw.zero_count       = getLong(kv, "Motor.yaw_zero_count", cfg.geometry.yaw.zero_count);
+    cfg.geometry.yaw.min_deg          = getDouble(kv, "Motor.yaw_min_deg", cfg.geometry.yaw.min_deg);
+    cfg.geometry.yaw.max_deg          = getDouble(kv, "Motor.yaw_max_deg", cfg.geometry.yaw.max_deg);
+    cfg.geometry.pitch.counts_per_deg = getDouble(kv, "Motor.pitch_counts_per_deg", cfg.geometry.pitch.counts_per_deg);
+    cfg.geometry.pitch.zero_count     = getLong(kv, "Motor.pitch_zero_count", cfg.geometry.pitch.zero_count);
+    cfg.geometry.pitch.min_deg        = getDouble(kv, "Motor.pitch_min_deg", cfg.geometry.pitch.min_deg);
+    cfg.geometry.pitch.max_deg        = getDouble(kv, "Motor.pitch_max_deg", cfg.geometry.pitch.max_deg);
+
+    // [Scan]: scan-grid source (explicit CSV file, else generated).
+    cfg.scan.grid_file     = get(kv, "Scan.grid_file", cfg.scan.grid_file);
+    cfg.scan.yaw_intervals = getInt(kv, "Scan.yaw_intervals", cfg.scan.yaw_intervals);
+    cfg.scan.yaw_min_deg   = getDouble(kv, "Scan.yaw_min_deg", cfg.scan.yaw_min_deg);
+    cfg.scan.yaw_max_deg   = getDouble(kv, "Scan.yaw_max_deg", cfg.scan.yaw_max_deg);
+    cfg.scan.pitch_levels  = get(kv, "Scan.pitch_levels", cfg.scan.pitch_levels);
+
     // ---- validation ----
     if (cfg.general.image_interval <= 0)
         throw std::runtime_error("General.image_interval must be > 0 (got " +

src/core/Geometry.cpp

@@ -0,0 +1,18 @@
+#include "fgc/Geometry.h"
+
+#include <algorithm>
+#include <cmath>
+
+namespace fgc {
+
+long AxisMap::toCounts(double deg) const {
+    double clamped = std::clamp(deg, min_deg, max_deg);
+    return std::lround(static_cast<double>(zero_count) + clamped * counts_per_deg);
+}
+
+double AxisMap::toDeg(long counts) const {
+    if (std::abs(counts_per_deg) < 1e-9) return 0.0;
+    return (static_cast<double>(counts) - static_cast<double>(zero_count)) / counts_per_deg;
+}
+
+}  // namespace fgc

src/core/ScanGrid.cpp

@@ -0,0 +1,113 @@
+#include "fgc/ScanGrid.h"
+
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+namespace fgc {
+
+namespace {
+
+std::string trim(const std::string& s) {
+    size_t b = s.find_first_not_of(" \t\r\n");
+    if (b == std::string::npos) return "";
+    size_t e = s.find_last_not_of(" \t\r\n");
+    return s.substr(b, e - b + 1);
+}
+
+std::vector<double> parseDoubleList(const std::string& csv) {
+    std::vector<double> out;
+    size_t start = 0;
+    while (start <= csv.size()) {
+        size_t comma = csv.find(',', start);
+        std::string tok =
+            trim(csv.substr(start, comma == std::string::npos ? std::string::npos : comma - start));
+        if (!tok.empty()) out.push_back(std::stod(tok));
+        if (comma == std::string::npos) break;
+        start = comma + 1;
+    }
+    return out;
+}
+
+}  // namespace
+
+std::vector<ScanPoint> parseScanCsv(const std::string& text) {
+    std::vector<ScanPoint> pts;
+    std::istringstream in(text);
+    std::string line;
+    int lineno = 0;
+    while (std::getline(in, line)) {
+        ++lineno;
+        std::string t = trim(line);
+        if (t.empty() || t[0] == '#') continue;
+        size_t comma = t.find(',');
+        if (comma == std::string::npos)
+            throw std::runtime_error("scan grid line " + std::to_string(lineno) +
+                                     ": expected 'yaw_deg,pitch_deg', got '" + t + "'");
+        try {
+            ScanPoint p;
+            p.yaw_deg   = std::stod(trim(t.substr(0, comma)));
+            p.pitch_deg = std::stod(trim(t.substr(comma + 1)));
+            pts.push_back(p);
+        } catch (const std::exception&) {
+            throw std::runtime_error("scan grid line " + std::to_string(lineno) +
+                                     ": non-numeric coordinate in '" + t + "'");
+        }
+    }
+    return pts;
+}
+
+std::vector<ScanPoint> generateScanGrid(const ScanConfig& cfg) {
+    std::vector<ScanPoint> pts;
+    std::vector<double> pitches = parseDoubleList(cfg.pitch_levels);
+    if (pitches.empty()) pitches.push_back(0.0);
+    int n = cfg.yaw_intervals;
+    if (n < 1) n = 1;
+    // pitch-major, yaw inner (min -> max); the runtime cursor ping-pongs the whole list.
+    for (double pitch : pitches) {
+        for (int i = 0; i < n; ++i) {
+            double frac = n > 1 ? static_cast<double>(i) / (n - 1) : 0.0;
+            ScanPoint p;
+            p.yaw_deg   = cfg.yaw_min_deg + frac * (cfg.yaw_max_deg - cfg.yaw_min_deg);
+            p.pitch_deg = pitch;
+            pts.push_back(p);
+        }
+    }
+    return pts;
+}
+
+ScanGrid ScanGrid::fromConfig(const ScanConfig& cfg) {
+    if (!cfg.grid_file.empty()) {
+        std::ifstream f(cfg.grid_file);
+        if (!f) throw std::runtime_error("cannot open scan grid file: " + cfg.grid_file);
+        std::stringstream buf;
+        buf << f.rdbuf();
+        auto pts = parseScanCsv(buf.str());
+        if (pts.empty())
+            throw std::runtime_error("scan grid file has no waypoints: " + cfg.grid_file);
+        return ScanGrid(std::move(pts));
+    }
+    return ScanGrid(generateScanGrid(cfg));
+}
+
+void ScanGrid::advance() {
+    if (points_.size() <= 1) return;
+    if (forward_) {
+        if (index_ + 1 < points_.size()) {
+            ++index_;
+        } else {
+            forward_ = false;
+            --index_;
+        }
+    } else {
+        if (index_ > 0) {
+            --index_;
+        } else {
+            forward_ = true;
+            ++index_;
+        }
+    }
+}
+
+}  // namespace fgc

src/core/TelemetryParser.cpp

@@ -1,5 +1,6 @@
 #include "fgc/TelemetryParser.h"
 
+#include <sstream>
 #include <vector>
 
 namespace fgc {
@@ -21,35 +22,75 @@ std::vector<std::string> split(const std::string& s, char delim) {
     return out;
 }
 
+AxisState stateFromChar(char c) {
+    switch (c) {
+        case 'B': case 'b': return AxisState::Boot;
+        case 'R': case 'r': return AxisState::Reset;
+        case 'H': case 'h': return AxisState::Homing;
+        case 'A': case 'a': return AxisState::Ready;
+        case 'E': case 'e': return AxisState::Error;
+        default:            return AxisState::Unknown;
+    }
+}
+
 }  // namespace
 
-std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
+std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg) {
-    if (line.empty() || line[0] != '$') return std::nullopt;
+    // seg looks like "A,982,969,80084000,0,8,8,Se" (the part after "Y:").
-
+    std::vector<std::string> f = split(seg, ',');
-    std::vector<std::string> f = split(line, ';');
+    if (f.size() < 7 || f[0].empty()) return std::nullopt;  // flags field (index 7) is optional
-    // Need indices 0..11: '$' marker + 11 data fields.
-    if (f.size() < 12) return std::nullopt;
-
     try {
-        MotorTelemetry t;
+        AxisTelemetry a;
-        t.encoder        = std::stoi(f[1]);
+        a.state      = stateFromChar(f[0][0]);
-        t.encoder_err    = std::stoi(f[2]);
+        a.xactual    = std::stol(f[1]);
-        t.sgt_val        = std::stoi(f[3]);
+        a.xenc       = std::stol(f[2]);
-        t.sgt_stat       = std::stoi(f[4]);
+        a.drv_status = static_cast<unsigned>(std::stoul(f[3], nullptr, 16));
-        t.is_moving      = std::stoi(f[5]);
+        a.sg         = std::stoi(f[4]);
-        t.control_status = std::stoi(f[6]);
+        a.cs         = std::stoi(f[5]);
-        t.heading        = std::stof(f[7]);
+        a.pwm        = std::stoi(f[6]);
-        t.deviation_warn = std::stoi(f[8]);
+        if (f.size() > 7) {
-        // NOTE: field order is humidity (index 9) BEFORE temperature (index 10),
+            for (char c : f[7]) {
-        // matching the controller firmware. See docs/known-issues.md #6 - do not
+                switch (c) {
-        // swap without confirming the firmware output.
+                    case 'S': a.standstill = true; break;
-        t.humidity       = std::stoi(f[9]);
+                    case 's': a.stall      = true; break;
-        t.temperature    = std::stoi(f[10]);
+                    case 'o': case 'O': a.overtemp = true; break;
-        t.fan_pwm        = std::stoi(f[11]);
+                    case 'L': a.endstop_l  = true; break;
-        return t;
+                    case 'R': a.endstop_r  = true; break;
+                    default: break;  // 'e' (eeprom) and unknown letters ignored
+                }
+            }
+        }
+        return a;
     } catch (const std::exception&) {
         return std::nullopt;
     }
 }
 
+std::optional<MotorTelemetry> parseTelemetryLine(const std::string& line) {
+    // Expect: ST Y:<...> [P:<...>]
+    std::istringstream in(line);
+    std::string tok;
+    if (!(in >> tok) || tok != "ST") return std::nullopt;
+
+    MotorTelemetry t;
+    bool saw_yaw = false;
+    while (in >> tok) {
+        // tok is "Y:..." or "P:..."
+        auto colon = tok.find(':');
+        if (colon == std::string::npos) return std::nullopt;
+        char axis = tok[0];
+        auto seg  = parseAxisSegment(tok.substr(colon + 1));
+        if (!seg) return std::nullopt;
+        if (axis == 'Y' || axis == 'y') {
+            t.yaw   = *seg;
+            saw_yaw = true;
+        } else if (axis == 'P' || axis == 'p') {
+            t.pitch         = *seg;
+            t.pitch_present = true;
+        }
+    }
+    if (!saw_yaw) return std::nullopt;
+    return t;
+}
+
 }  // namespace fgc

src/mqtt/MqttControlChannel.cpp

@@ -118,6 +118,7 @@ void MqttControlChannel::publishStatus(int code) {
 void MqttControlChannel::publishCamEvent(const CamEvent& e) {
     std::string payload = "{ \"fwt\":\"" + e.tower + "\" ,\"cam\":\"" + e.camera +
                           "\", \"hdg\":" + std::to_string(e.heading_decideg) +
+                          ", \"pit\":" + std::to_string(e.pitch_decideg) +
                           ", \"time\":" + std::to_string(e.timestamp_ms) + " }";
     publish(topic_cam_event_, payload);
 }

src/serial/SerialMotorController.cpp

@@ -36,15 +36,31 @@ struct SerialMotorController::Impl {
                 std::istream is(&buffer);
                 std::string line;
                 std::getline(is, line);
+                // Strip a trailing CR (firmware may send CRLF).
+                if (!line.empty() && line.back() == '\r') line.pop_back();
+                dispatchLine(line);
+                doRead();
+            });
+    }
+
+    // Route an inbound line: ST -> telemetry; OK/ERR -> command ack; everything
+    // else (DG/DUMP/BOOT/IOIN/GSTAT...) is async output we just trace.
+    void dispatchLine(const std::string& line) {
+        if (line.rfind("ST ", 0) == 0 || line == "ST") {
             if (auto t = parseTelemetryLine(line)) {
                 LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
                 std::lock_guard<std::mutex> lock(mutex);
                 latest = *t;
             } else {
-                    LOG_TRACE_CAT(LogCat::Serial) << "RX(unparsed) " << line;
+                LOG_TRACE_CAT(LogCat::Serial) << "RX(unparsed ST) " << line;
+            }
+        } else if (line.rfind("ERR", 0) == 0) {
+            LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
+            LOG_WARN << "Motor controller: " << line;
+        } else if (!line.empty()) {
+            // OK acks and other async output (DG/DUMP/BOOT/...).
+            LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
         }
-                doRead();
-            });
     }
 };
 
@@ -89,7 +105,8 @@ void SerialMotorController::stop() {
 void SerialMotorController::sendCommand(const std::string& cmd) {
     if (!impl_->connected) return;
     LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
-    auto data = std::make_shared<std::string>(cmd);
+    // The firmware requires a newline terminator on every command.
+    auto data = std::make_shared<std::string>(cmd + "\n");
     boost::asio::async_write(impl_->serial, boost::asio::buffer(*data),
                              [data](const boost::system::error_code& ec, std::size_t) {
                                  if (ec) LOG_WARN << "Serial write failed: " << ec.message();

tests/CMakeLists.txt

@@ -25,6 +25,8 @@ add_executable(fgc_tests
   test_command.cpp
   test_scheduler.cpp
   test_logger.cpp
+  test_geometry.cpp
+  test_scangrid.cpp
 )
 target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
 

tests/test_geometry.cpp

@@ -0,0 +1,32 @@
+#include <doctest/doctest.h>
+
+#include "fgc/Geometry.h"
+
+using namespace fgc;
+
+TEST_CASE("AxisMap converts degrees and counts with an offset") {
+    AxisMap m{983.33, 500000, -90.0, 90.0};
+
+    CHECK(m.toCounts(0.0) == 500000);
+    CHECK(m.toCounts(90.0) == 588500);  // lround(500000 + 90*983.33)
+    CHECK(m.toDeg(500000) == doctest::Approx(0.0));
+    CHECK(m.toDeg(m.toCounts(45.0)) == doctest::Approx(45.0).epsilon(0.001));
+}
+
+TEST_CASE("AxisMap clamps to the soft range on the way out") {
+    AxisMap m{10.0, 0, -90.0, 90.0};
+    CHECK(m.toCounts(200.0) == 900);   // clamped to +90
+    CHECK(m.toCounts(-200.0) == -900); // clamped to -90
+    CHECK(m.toCounts(45.0) == 450);
+}
+
+TEST_CASE("AxisMap supports a negative (flipped) direction") {
+    AxisMap m{-10.0, 1000, -90.0, 90.0};
+    CHECK(m.toCounts(10.0) == 900);
+    CHECK(m.toDeg(900) == doctest::Approx(10.0));
+}
+
+TEST_CASE("AxisMap toDeg guards against a zero scale") {
+    AxisMap m{0.0, 0, -90.0, 90.0};
+    CHECK(m.toDeg(12345) == doctest::Approx(0.0));
+}

tests/test_scangrid.cpp

@@ -0,0 +1,53 @@
+#include <doctest/doctest.h>
+
+#include "fgc/ScanGrid.h"
+
+using namespace fgc;
+
+TEST_CASE("parseScanCsv reads waypoints and skips comments/blanks") {
+    auto pts = parseScanCsv("# header\n\n-90,10\n0, 20 \n90,30\n");
+    REQUIRE(pts.size() == 3);
+    CHECK(pts[0].yaw_deg == doctest::Approx(-90.0));
+    CHECK(pts[1].pitch_deg == doctest::Approx(20.0));
+    CHECK(pts[2].yaw_deg == doctest::Approx(90.0));
+}
+
+TEST_CASE("parseScanCsv throws on a malformed line") {
+    CHECK_THROWS(parseScanCsv("0,0\nnonsense\n"));
+    CHECK_THROWS(parseScanCsv("1,abc\n"));
+}
+
+TEST_CASE("generateScanGrid produces yaw_intervals x pitch_levels points") {
+    ScanConfig c;
+    c.yaw_intervals = 3;
+    c.yaw_min_deg = -90;
+    c.yaw_max_deg = 90;
+    c.pitch_levels = "10,30";
+    auto pts = generateScanGrid(c);
+    REQUIRE(pts.size() == 6);
+    // pitch-major, yaw inner (min -> max)
+    CHECK(pts[0].yaw_deg == doctest::Approx(-90.0));
+    CHECK(pts[0].pitch_deg == doctest::Approx(10.0));
+    CHECK(pts[1].yaw_deg == doctest::Approx(0.0));
+    CHECK(pts[2].yaw_deg == doctest::Approx(90.0));
+    CHECK(pts[3].pitch_deg == doctest::Approx(30.0));
+}
+
+TEST_CASE("ScanGrid cursor ping-pongs at both ends") {
+    ScanGrid g({{0, 0}, {1, 0}, {2, 0}});
+    CHECK(g.current().yaw_deg == doctest::Approx(0.0));
+    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));
+    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(2.0));
+    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));  // reversed
+    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(0.0));
+    g.advance(); CHECK(g.current().yaw_deg == doctest::Approx(1.0));  // reversed again
+}
+
+TEST_CASE("ScanGrid single point and empty are safe") {
+    ScanGrid one({{5, 5}});
+    one.advance();
+    CHECK(one.current().yaw_deg == doctest::Approx(5.0));
+
+    ScanGrid none;
+    CHECK(none.empty());
+}

tests/test_scheduler.cpp

@@ -46,67 +46,112 @@ struct FakeChannel : IControlChannel {
     }
 };
 
+// Simple linear map: 10 counts/deg, zero at 0. Yaw clamps -90..90, pitch 0..60.
+Geometry testGeometry() {
+    Geometry g;
+    g.yaw   = {10.0, 0, -90.0, 90.0};
+    g.pitch = {10.0, 0, 0.0, 60.0};
+    return g;
+}
+
+// Mark both axes settled at the scheduler's current target.
+void settleAt(FakeMotor& m, long yaw, long pitch) {
+    m.tel.yaw.xenc = yaw;     m.tel.yaw.standstill = true;     m.tel.yaw.state = AxisState::Ready;
+    m.tel.pitch.xenc = pitch; m.tel.pitch.standstill = true;   m.tel.pitch.state = AxisState::Ready;
+    m.tel.pitch_present = true;
+}
+
 }  // namespace
 
-TEST_CASE("CaptureScheduler drives the auto-sweep capture cycle") {
+TEST_CASE("CaptureScheduler sweeps the scan grid with MOVE + settle + trigger") {
     long long   clock = 0;
     FakeMotor   motor;
     FakeCamera  cam;
     FakeChannel chan;
+    ScanGrid    grid({{0.0, 0.0}, {90.0, 0.0}});  // -> yaw counts 0, then 900
 
-    CaptureScheduler sch(motor, cam, chan, 1.0 /*img/s -> 1000ms*/, [&] { return clock; });
+    CaptureScheduler sch(motor, cam, chan, 1.0 /*img/s -> 1000ms*/, testGeometry(), grid,
+                         [&] { return clock; });
     sch.setCaptureActive(true);
 
     chan.next.control_code_available = true;
     chan.next.control_code = 0;
-    motor.tel.is_moving = 1;
 
-    // Before the interval elapses: nothing happens, but status is echoed.
+    // Before the interval elapses: nothing moves, but status is echoed.
     clock = 500;
     sch.tick();
     CHECK(motor.cmds.empty());
     CHECK(chan.last_status == 0);
 
-    // After the interval, while moving: stop command issued, timer reset.
+    // After the interval: MOVE to the first grid point (0,0) is issued.
     clock = 1600;
     sch.tick();
     REQUIRE(motor.cmds.size() == 1);
-    CHECK(motor.cmds[0] == "p");
+    CHECK(motor.cmds[0] == "MOVE 0,0");
+    CHECK(sch.moving());
+    CHECK(cam.triggers == 0);  // not yet settled
 
-    // >100ms later, still moving: camera fires.
+    // Report standstill at the target -> camera fires, cursor advances.
-    clock = 1750;
+    settleAt(motor, 0, 0);
+    clock = 1700;
     sch.tick();
     CHECK(cam.triggers == 1);
+    CHECK_FALSE(sch.moving());
+
+    // Next interval: MOVE to the second grid point (90 deg -> 900 counts).
+    clock = 2800;
+    sch.tick();
+    CHECK(motor.cmds.back() == "MOVE 900,0");
 }
 
-TEST_CASE("CaptureScheduler ControlCode 1 drives to target heading") {
+TEST_CASE("CaptureScheduler ControlCode 1 drives yaw to the target heading") {
     long long   clock = 0;
     FakeMotor   motor;
     FakeCamera  cam;
     FakeChannel chan;
-    CaptureScheduler sch(motor, cam, chan, 1.0, [&] { return clock; });
+    ScanGrid    grid({{0.0, 0.0}});
+    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
     sch.setCaptureActive(true);
-    motor.tel.is_moving = 1;
 
     chan.next.control_code_available = true;
     chan.next.control_code = 1;
     chan.next.heading_available = true;
-    chan.next.target_heading = "180";
+    chan.next.target_heading = "45";  // 45 deg * 10 = 450 counts
 
     clock = 1600;
     sch.tick();
     REQUIRE(!motor.cmds.empty());
-    CHECK(motor.cmds.back() == "kd180");
+    CHECK(motor.cmds.back() == "MOVE 450,0");
     CHECK(sch.controlCode() == 1);
 }
 
+TEST_CASE("CaptureScheduler clamps the target heading to the soft range") {
+    long long   clock = 0;
+    FakeMotor   motor;
+    FakeCamera  cam;
+    FakeChannel chan;
+    ScanGrid    grid({{0.0, 0.0}});
+    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
+    sch.setCaptureActive(true);
+
+    chan.next.control_code_available = true;
+    chan.next.control_code = 1;
+    chan.next.heading_available = true;
+    chan.next.target_heading = "200";  // clamped to yaw max 90 deg -> 900 counts
+
+    clock = 1600;
+    sch.tick();
+    CHECK(motor.cmds.back() == "MOVE 900,0");
+}
+
 TEST_CASE("CaptureScheduler stays idle when capture inactive") {
     long long   clock = 0;
     FakeMotor   motor;
     FakeCamera  cam;
     FakeChannel chan;
-    CaptureScheduler sch(motor, cam, chan, 1.0, [&] { return clock; });
+    ScanGrid    grid({{0.0, 0.0}});
-    motor.tel.is_moving = 1;  // moving, but capture not active
+    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
+    // capture not active
 
     clock = 5000;
     sch.tick();

tests/test_telemetry.cpp

@@ -4,27 +4,48 @@
 
 using namespace fgc;
 
-TEST_CASE("parseTelemetryLine reads a valid record") {
+TEST_CASE("parseTelemetryLine reads a two-axis ST line") {
-    auto t = parseTelemetryLine("$;100;2;3;4;1;5;123.5;0;55;21;200;");
+    auto t = parseTelemetryLine("ST Y:A,982,969,80084000,0,8,8,Se P:H,500000,-4,80084000,0,8,8,SL");
     REQUIRE(t.has_value());
-    CHECK(t->encoder == 100);
+
-    CHECK(t->is_moving == 1);
+    CHECK(t->yaw.state == AxisState::Ready);
-    CHECK(t->heading == doctest::Approx(123.5f));
+    CHECK(t->yaw.xactual == 982);
-    // field order: humidity (index 9) before temperature (index 10)
+    CHECK(t->yaw.xenc == 969);
-    CHECK(t->humidity == 55);
+    CHECK(t->yaw.drv_status == 0x80084000u);
-    CHECK(t->temperature == 21);
+    CHECK(t->yaw.cs == 8);
-    CHECK(t->fan_pwm == 200);
+    CHECK(t->yaw.standstill == true);
+    CHECK(t->yaw.moving() == false);
+
+    REQUIRE(t->pitch_present);
+    CHECK(t->pitch.state == AxisState::Homing);
+    CHECK(t->pitch.xactual == 500000);
+    CHECK(t->pitch.xenc == -4);
+    CHECK(t->pitch.standstill == true);
+    CHECK(t->pitch.endstop_l == true);
+    CHECK(t->pitch.moving() == true);  // Homing counts as moving
 }
 
-TEST_CASE("parseTelemetryLine rejects malformed lines") {
+TEST_CASE("parseTelemetryLine reads a yaw-only ST line (no flags field)") {
-    CHECK_FALSE(parseTelemetryLine("garbage").has_value());
+    auto t = parseTelemetryLine("ST Y:R,10,12,00000000,0,0,0");
+    REQUIRE(t.has_value());
+    CHECK(t->yaw.state == AxisState::Reset);
+    CHECK(t->yaw.xenc == 12);
+    CHECK(t->yaw.standstill == false);
+    CHECK_FALSE(t->pitch_present);
+}
+
+TEST_CASE("parseTelemetryLine rejects non-ST and malformed lines") {
     CHECK_FALSE(parseTelemetryLine("").has_value());
-    CHECK_FALSE(parseTelemetryLine("$;1;2;3").has_value());                 // too few
+    CHECK_FALSE(parseTelemetryLine("OK").has_value());
-    CHECK_FALSE(parseTelemetryLine("$;a;2;3;4;5;6;7;8;9;10;11;").has_value()); // bad int
+    CHECK_FALSE(parseTelemetryLine("ERR bad axis").has_value());
+    CHECK_FALSE(parseTelemetryLine("DG DONE").has_value());
+    CHECK_FALSE(parseTelemetryLine("$;100;2;3;4;1;5;123.5;0;55;21;200;").has_value());  // old format
+    CHECK_FALSE(parseTelemetryLine("ST Y:A,982").has_value());          // too few fields
+    CHECK_FALSE(parseTelemetryLine("ST P:A,1,2,0,0,0,0").has_value());  // no yaw segment
 }
 
-TEST_CASE("parseTelemetryLine tolerates trailing CR/LF") {
+TEST_CASE("parseTelemetryLine tolerates trailing CR") {
-    auto t = parseTelemetryLine("$;1;2;3;4;0;6;7.0;8;9;10;11;\r\n");
+    auto t = parseTelemetryLine("ST Y:A,1,2,0,0,0,0,S\r");
     REQUIRE(t.has_value());
-    CHECK(t->fan_pwm == 11);
+    CHECK(t->yaw.standstill == true);
 }