Migrate host to the current firmware serial protocol (counts/MOVE/ST)
This commit is contained in:
parent
90f689c829
commit
14707c62ae
GPG Key ID:
D4A6C394DF13F1D7
|
|
@ -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
|
||||
|
|
|
|||
10
README.md
10
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
Can't render this file because it contains an unexpected character in line 1 and column 40.
|
|
|
@ -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] 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<heading>` | 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 <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).
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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. |
|
||||
| `1` | **Directed.** On each interval, send `kd<target_HDG>` 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/<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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// 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 <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
|
||||
// MQTT-supplied target heading ("kd<heading>").
|
||||
// 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<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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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).
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
ImagePipeline(IControlChannel& channel, std::function<float()> heading, Params params);
|
||||
// orientation: returns the current gimbal yaw+pitch (degrees) for CamEvent.
|
||||
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_;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
// (a trailing ';' is tolerated). Returns nullopt if the line does not start
|
||||
// with '$', has too few fields, or a field fails to convert.
|
||||
// where <state> is B/R/H/A/E and <flags> is an optional run of status letters
|
||||
// (S=standstill, s=stall, o/O=overtemp, L/R=endstops, e=eeprom). Returns nullopt
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// gimbal is continuously sweeping: heading advances on each telemetry read and
|
||||
// is_moving stays 1 so the capture cycle fires. "kd<heading>" sets the target.
|
||||
// Simulated motor controller for development without hardware. Models the new
|
||||
// firmware's encoder-count protocol: HOME makes both axes READY; MOVE sets a
|
||||
// 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) {
|
||||
try {
|
||||
tel_.heading = std::stof(cmd.substr(2));
|
||||
} catch (const std::exception&) {}
|
||||
std::istringstream in(cmd);
|
||||
std::string verb;
|
||||
in >> verb;
|
||||
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
|
||||
if (tel_.heading >= 360.f) tel_.heading -= 360.f;
|
||||
tel_.is_moving = 1;
|
||||
tel_.temperature = 20;
|
||||
tel_.humidity = 50;
|
||||
tel_.fan_pwm = 0;
|
||||
return tel_;
|
||||
step(yaw_, yaw_target_);
|
||||
step(pitch_, pitch_target_);
|
||||
MotorTelemetry t;
|
||||
t.yaw = yaw_;
|
||||
t.pitch = pitch_;
|
||||
t.pitch_present = true;
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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)";
|
||||
motor->sendCommand("r");
|
||||
std::this_thread::sleep_for(500ms);
|
||||
motor->sendCommand("e");
|
||||
std::this_thread::sleep_for(500ms);
|
||||
motor->sendCommand("q");
|
||||
std::this_thread::sleep_for(60s);
|
||||
motor->sendCommand("y");
|
||||
std::this_thread::sleep_for(500ms);
|
||||
motor->sendCommand("ud56"); // intervals per turn
|
||||
LOG_INFO << "Running gimbal init sequence (enable + home)";
|
||||
motor->sendCommand("ENABLE Y");
|
||||
motor->sendCommand("ENABLE P");
|
||||
motor->sendCommand("HOME"); // homes all axes; firmware runs it non-blocking
|
||||
|
||||
auto allReady = [](const MotorTelemetry& t) {
|
||||
return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
|
||||
};
|
||||
|
||||
// The axes may already be READY (EEPROM fast-path), so first wait briefly
|
||||
// for homing to actually begin (an axis leaves READY) before waiting for
|
||||
// it to finish - otherwise we'd see the residual READY and return early.
|
||||
const auto t0 = std::chrono::steady_clock::now();
|
||||
while (running && std::chrono::steady_clock::now() < t0 + 3s) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
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,
|
||||
cfg.image_rate());
|
||||
try {
|
||||
grid = ScanGrid::fromConfig(cfg.scan);
|
||||
} catch (const std::exception& e) {
|
||||
LOG_WARN << "scan grid load failed (" << e.what() << "); auto-sweep disabled";
|
||||
}
|
||||
LOG_INFO << "scan grid: " << grid.size() << " waypoints";
|
||||
scheduler = std::make_unique<CaptureScheduler>(*motor, *camera, *channel, cfg.image_rate(),
|
||||
cfg.geometry, grid);
|
||||
|
||||
motor->start();
|
||||
camera->open();
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
//
|
||||
// NOTE: this preserves the original behaviour of triggering while the gimbal
|
||||
// reports is_moving == 1. That looks counter-intuitive (one might expect to
|
||||
// trigger once stopped). See docs/known-issues.md #7 - kept as-is pending
|
||||
// confirmation of the firmware's is_moving semantics.
|
||||
bool shouldTrigger(const MotorTelemetry& t) { return t.is_moving == 1; }
|
||||
// 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
|
||||
// deadband (~200 counts).
|
||||
constexpr long kSettleTolCounts = 600;
|
||||
|
||||
} // namespace
|
||||
|
||||
CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
|
||||
IControlChannel& channel, double image_rate,
|
||||
std::function<long long()> now_ms)
|
||||
IControlChannel& channel, double image_rate, Geometry geometry,
|
||||
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 (t.is_moving == 1) {
|
||||
resetTimer();
|
||||
trigger_after_stopping_ = true;
|
||||
if (!moving_) {
|
||||
// Issue the next move once the interval has elapsed.
|
||||
if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;
|
||||
|
||||
if (control_code_ == 0) {
|
||||
LOG_TRACE_CAT(LogCat::Control) << "motor cmd \"p\" (auto sweep)";
|
||||
motor_.sendCommand("p");
|
||||
} else { // control_code_ == 1
|
||||
LOG_TRACE_CAT(LogCat::Control) << "motor cmd \"kd" << target_heading_ << '"';
|
||||
motor_.sendCommand("kd" + target_heading_);
|
||||
if (grid_.empty()) return;
|
||||
const ScanPoint& p = grid_.current();
|
||||
yaw_target_ = geometry_.yaw.toCounts(p.yaw_deg);
|
||||
pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
|
||||
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) {
|
||||
if (shouldTrigger(t)) {
|
||||
LOG_TRACE_CAT(LogCat::Control) << "trigger decision: is_moving=" << t.is_moving;
|
||||
// Moving: trigger once both axes have settled at the target.
|
||||
if (settledAt(t)) {
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 " +
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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) {
|
||||
if (line.empty() || line[0] != '$') return std::nullopt;
|
||||
|
||||
std::vector<std::string> f = split(line, ';');
|
||||
// Need indices 0..11: '$' marker + 11 data fields.
|
||||
if (f.size() < 12) return std::nullopt;
|
||||
|
||||
std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg) {
|
||||
// seg looks like "A,982,969,80084000,0,8,8,Se" (the part after "Y:").
|
||||
std::vector<std::string> f = split(seg, ',');
|
||||
if (f.size() < 7 || f[0].empty()) return std::nullopt; // flags field (index 7) is optional
|
||||
try {
|
||||
MotorTelemetry t;
|
||||
t.encoder = std::stoi(f[1]);
|
||||
t.encoder_err = std::stoi(f[2]);
|
||||
t.sgt_val = std::stoi(f[3]);
|
||||
t.sgt_stat = std::stoi(f[4]);
|
||||
t.is_moving = std::stoi(f[5]);
|
||||
t.control_status = std::stoi(f[6]);
|
||||
t.heading = std::stof(f[7]);
|
||||
t.deviation_warn = std::stoi(f[8]);
|
||||
// NOTE: field order is humidity (index 9) BEFORE temperature (index 10),
|
||||
// matching the controller firmware. See docs/known-issues.md #6 - do not
|
||||
// swap without confirming the firmware output.
|
||||
t.humidity = std::stoi(f[9]);
|
||||
t.temperature = std::stoi(f[10]);
|
||||
t.fan_pwm = std::stoi(f[11]);
|
||||
return t;
|
||||
AxisTelemetry a;
|
||||
a.state = stateFromChar(f[0][0]);
|
||||
a.xactual = std::stol(f[1]);
|
||||
a.xenc = std::stol(f[2]);
|
||||
a.drv_status = static_cast<unsigned>(std::stoul(f[3], nullptr, 16));
|
||||
a.sg = std::stoi(f[4]);
|
||||
a.cs = std::stoi(f[5]);
|
||||
a.pwm = std::stoi(f[6]);
|
||||
if (f.size() > 7) {
|
||||
for (char c : f[7]) {
|
||||
switch (c) {
|
||||
case 'S': a.standstill = true; break;
|
||||
case 's': a.stall = true; break;
|
||||
case 'o': case 'O': a.overtemp = true; break;
|
||||
case 'L': a.endstop_l = true; break;
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
|
@ -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());
|
||||
}
|
||||
|
|
@ -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.
|
||||
clock = 1750;
|
||||
// Report standstill at the target -> camera fires, cursor advances.
|
||||
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; });
|
||||
motor.tel.is_moving = 1; // moving, but capture not active
|
||||
ScanGrid grid({{0.0, 0.0}});
|
||||
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
||||
// capture not active
|
||||
|
||||
clock = 5000;
|
||||
sch.tick();
|
||||
|
|
|
|||
|
|
@ -4,27 +4,48 @@
|
|||
|
||||
using namespace fgc;
|
||||
|
||||
TEST_CASE("parseTelemetryLine reads a valid record") {
|
||||
auto t = parseTelemetryLine("$;100;2;3;4;1;5;123.5;0;55;21;200;");
|
||||
TEST_CASE("parseTelemetryLine reads a two-axis ST line") {
|
||||
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->heading == doctest::Approx(123.5f));
|
||||
// field order: humidity (index 9) before temperature (index 10)
|
||||
CHECK(t->humidity == 55);
|
||||
CHECK(t->temperature == 21);
|
||||
CHECK(t->fan_pwm == 200);
|
||||
|
||||
CHECK(t->yaw.state == AxisState::Ready);
|
||||
CHECK(t->yaw.xactual == 982);
|
||||
CHECK(t->yaw.xenc == 969);
|
||||
CHECK(t->yaw.drv_status == 0x80084000u);
|
||||
CHECK(t->yaw.cs == 8);
|
||||
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") {
|
||||
CHECK_FALSE(parseTelemetryLine("garbage").has_value());
|
||||
TEST_CASE("parseTelemetryLine reads a yaw-only ST line (no flags field)") {
|
||||
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("$;a;2;3;4;5;6;7;8;9;10;11;").has_value()); // bad int
|
||||
CHECK_FALSE(parseTelemetryLine("OK").has_value());
|
||||
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") {
|
||||
auto t = parseTelemetryLine("$;1;2;3;4;0;6;7.0;8;9;10;11;\r\n");
|
||||
TEST_CASE("parseTelemetryLine tolerates trailing CR") {
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue