Changed heading logic; added snapshot feature
This commit is contained in:
parent
cd8491dcc0
commit
1a572cb074
GPG Key ID:
D4A6C394DF13F1D7
|
|
@ -58,7 +58,8 @@ add_library(fgc_core STATIC
|
|||
src/core/HostMetrics.cpp
|
||||
src/core/TestRunner.cpp
|
||||
src/core/Calibration.cpp
|
||||
src/core/CalibrationRoutine.cpp
|
||||
src/core/HeadingFit.cpp
|
||||
src/core/PitchSetup.cpp
|
||||
src/core/MtiProtocol.cpp
|
||||
src/core/ImuMount.cpp
|
||||
src/core/CameraProfile.cpp
|
||||
|
|
|
|||
11
README.md
11
README.md
|
|
@ -125,12 +125,15 @@ While running, the program reads commands from stdin (one per line):
|
|||
| Command | Action |
|
||||
|---------|--------|
|
||||
| `start` / `stop` | Start / stop the capture cycle (the gimbal stays powered and homed). |
|
||||
| `gimbal move <yaw>,<pitch>` | Aim at an absolute heading/elevation in **degrees** (calibrated). |
|
||||
| `gimbal move <yaw>,<pitch>` | Aim at an absolute **true bearing** (0 = north) and elevation, in degrees. |
|
||||
| `gimbal home [y\|p]` | Run the endstop-finding home sequence. |
|
||||
| `gimbal dump` | Request a firmware state dump (shown in the gimbal `g` view). |
|
||||
| `gimbal diag [y\|p\|all]` | Run the firmware motor self-test (results in the activity strip + `logs/`). |
|
||||
| `gimbal calib` | IMU-referenced steps↔degrees calibration (auto-homes if needed; needs the IMU; applied to the session). |
|
||||
| `gimbal …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
|
||||
| `gimbal heading setup` | Tie yaw to true north by sighting known landmarks (TUI key `H`; **no IMU needed**). |
|
||||
| `gimbal heading offset <deg>` | Shift the whole yaw map by a known amount, and save it. |
|
||||
| `gimbal pitch setup` | Level pitch against gravity (auto-homes if needed; **needs the IMU**). |
|
||||
| `snapshot` | Take one image now and publish its CamEvent (TUI: `Space`; manual mode only). |
|
||||
| `gimbal …` | Other motor controls — `steps`/`nudge`/`jog`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
|
||||
| `gimbal raw "<command>"` | **Low-level:** send the quoted text to the firmware verbatim (newline added). Bypasses wrappers/unit conversion — use with care. |
|
||||
| `debug` | Toggle debug-level logging on/off. |
|
||||
| `set fps <n>` | Capture rate, in images per second. |
|
||||
|
|
@ -158,7 +161,7 @@ GUI validated. `standard` runs 10 Hz for daylight, `night` runs 1 Hz with long e
|
|||
[docs/configuration.md](docs/configuration.md).
|
||||
|
||||
The **Xsens MTi IMU** (set `[Features] enable_imu` + `[IMU] device`) drives the Sensors panel's live
|
||||
roll/pitch/yaw and powers `gimbal calib`; the **SHT41** (`[Features] enable_env` + `[Env] i2c_device`,
|
||||
roll/pitch/yaw and powers `gimbal pitch setup`; the **SHT41** (`[Features] enable_env` + `[Env] i2c_device`,
|
||||
or `--mock-env`) supplies the panel's ambient temperature and humidity — the MTi's device-internal
|
||||
temperature is expanded-view-only, so the main window shows one unambiguous temperature. In the TUI,
|
||||
`g` and `i` open full-screen **gimbal** and **sensors** views (the latter covering both the MTi and
|
||||
|
|
|
|||
|
|
@ -18,10 +18,16 @@ set(_vmb_hints
|
|||
"$ENV{VIMBA_X_HOME}"
|
||||
"$ENV{VIMBAX_HOME}"
|
||||
/opt/VimbaX
|
||||
/opt/VimbaX_2025-3
|
||||
/opt/VimbaX_2025-1
|
||||
/opt/VimbaX_2024-1
|
||||
/opt/vimbax
|
||||
/usr/local/VimbaX
|
||||
# The Allied Vision installer defaults to an unpacked directory under the
|
||||
# user's home rather than /opt, so a developer machine typically has it here.
|
||||
"$ENV{HOME}/Programs/VimbaX"
|
||||
"$ENV{HOME}/Programs/VimbaX_2025-3"
|
||||
"$ENV{HOME}/VimbaX"
|
||||
)
|
||||
|
||||
find_path(Vmb_INCLUDE_DIR
|
||||
|
|
|
|||
|
|
@ -3,6 +3,10 @@
|
|||
; Copy this file to "config.ini" and edit for your deployment:
|
||||
; cp config/config.example.ini config.ini
|
||||
;
|
||||
; WARNING: do not edit config.ini while the program is running. `gimbal heading
|
||||
; offset`, `gimbal heading setup` and `gimbal pitch setup` rewrite the [Motor]
|
||||
; block in place, so concurrent edits would be lost. Stop the program first.
|
||||
;
|
||||
; The real config.ini is gitignored because it contains credentials.
|
||||
; MQTT credentials may instead be supplied via the environment variables
|
||||
; FGC_MQTT_USER and FGC_MQTT_PW, which take precedence over the values here.
|
||||
|
|
@ -129,9 +133,10 @@ baud = 115200
|
|||
; gimbal - roll and pitch should read ~0. Tip the nose up - pitch goes
|
||||
; positive. Roll to the right - roll goes positive. Slew right - yaw increases.
|
||||
;
|
||||
; NOTE: `gimbal calib` fits IMU yaw against encoder counts, so a calibration
|
||||
; made before this was set has the mount's error baked in. Re-run it after
|
||||
; changing these.
|
||||
; NOTE: `gimbal pitch setup` fits IMU pitch against encoder counts, so a
|
||||
; calibration made before this was set has the mount's error baked in. Re-run it
|
||||
; after changing these. Yaw is unaffected - `gimbal heading setup` sights
|
||||
; landmarks and never consults the IMU.
|
||||
x = 0
|
||||
y = 0
|
||||
z = 0
|
||||
|
|
@ -147,40 +152,43 @@ i2c_addr = 0x44
|
|||
period_ms = 2000
|
||||
|
||||
[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.
|
||||
; Degrees<->encoder-counts maps, anchored on the mechanical centre found by
|
||||
; homing. The firmware speaks only in absolute encoder counts; these map them to
|
||||
; the heading/elevation degrees used by MQTT (target_HDG) and CamEvent.
|
||||
;
|
||||
; Homing finds both endstops, moves to the middle of that range and DEFINES the
|
||||
; midpoint as <axis>_center_count. Anchoring on the midpoint rather than one
|
||||
; endstop averages the error of both, and a large positive value keeps the whole
|
||||
; working range positive (the driver misbehaves on negative positions).
|
||||
;
|
||||
; Set the heading with `gimbal heading setup` (sight two known landmarks) or
|
||||
; `gimbal heading offset <deg>` to nudge it. Pitch uses `gimbal pitch setup`,
|
||||
; which needs the IMU.
|
||||
;
|
||||
; Travel limits are NOT configured here - they are derived from the firmware's
|
||||
; homed soft limits after every home, the only place they can be correct.
|
||||
;
|
||||
; Mechanical travel of this gimbal (between hard endstops, found at homing):
|
||||
; Yaw ~320 deg (0 .. 320; NOT a full revolution, cannot wrap)
|
||||
; Pitch 40 deg (-10 .. +30; -10 below horizon to +30 above)
|
||||
; Keep all scan waypoints inside these ranges or the move clamps short.
|
||||
; Yaw ~320 deg (NOT a full revolution: there is an unreachable arc)
|
||||
; Pitch 40 deg
|
||||
;
|
||||
; 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
|
||||
; Re-anchor the yaw zero from homing instead of the fixed yaw_zero_count above:
|
||||
; off (default) - use yaw_zero_count as written
|
||||
; low - the lower-count endstop becomes 0 deg, degrees rise toward
|
||||
; the higher-count endstop (which lands near +360)
|
||||
; high - the higher-count endstop becomes 0 deg, degrees rise toward
|
||||
; the lower-count endstop (which lands near +360)
|
||||
; Keeps yaw_counts_per_deg as the scale; only the zero + direction come from the
|
||||
; homed endstops, so the yaw origin is fixed by homing (no zero recalibration).
|
||||
; yaw_min_deg/yaw_max_deg are auto-set to 0..(homed range). Pitch is unaffected.
|
||||
yaw_home_zero = off
|
||||
; Pitch (physical endstops, -10..+30 deg = 40 deg travel). CALIBRATE counts_per_deg/zero_count.
|
||||
yaw_center_count = 500000
|
||||
; True bearing (0 = north) the gimbal points at when parked at the centre.
|
||||
yaw_heading_at_center = 0.0
|
||||
; Clearance kept clear of each hard endstop, in counts. Sent to the firmware,
|
||||
; which enforces it. Counts, not degrees: it must apply at homing time, before
|
||||
; any degree map is trusted.
|
||||
yaw_soft_margin_counts = 5000
|
||||
; Pitch (physical endstops). CALIBRATE with `gimbal pitch setup`.
|
||||
pitch_counts_per_deg = 8333.33
|
||||
pitch_zero_count = 0
|
||||
pitch_min_deg = -10
|
||||
pitch_max_deg = 30
|
||||
; Live encoder-tracking WARN: log a warning when an axis's |xactual - xenc|
|
||||
; exceeds this many counts (positional slip). 0 disables. Default 400 sits above
|
||||
; the firmware hold deadband (200) and below the DIAG peak limit (500).
|
||||
pitch_center_count = 500000
|
||||
; Elevation above level (+ = up) at the centre. This is NOT 0: the mechanical
|
||||
; centre is the middle of pitch travel, which sits well above the horizon.
|
||||
pitch_angle_at_center = 10.0
|
||||
pitch_soft_margin_counts = 5000
|
||||
; Live encoder-error WARN threshold in counts (0 = off).
|
||||
enc_error_warn_counts = 400
|
||||
|
||||
[Scan]
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@ The design separates **policy** (the control logic) from **mechanism** (the I/O
|
|||
| `IEnvSensor` | `Sht41EnvSensor` (SHT41 over I2C) | `MockEnvSensor` (synthetic temp/humidity) |
|
||||
|
||||
Both sensors are optional (`[Features] enable_imu` / `enable_env`) and feed the Sensors panel: the IMU
|
||||
supplies orientation and the IMU-referenced `gimbal calib`, the SHT41 the ambient temperature and
|
||||
humidity (also published on the MQTT `Env` topic). Long-running operations (`gimbal calib` on a worker thread, `gimbal diag` captured from
|
||||
supplies orientation and the IMU-referenced `gimbal pitch setup`, the SHT41 the ambient temperature and
|
||||
humidity. Long-running operations (`gimbal pitch setup` on a worker thread, `gimbal diag` captured from
|
||||
the firmware) publish progress + results into the `UiSnapshot` activity strip, polled on the control
|
||||
thread so all geometry/state mutation stays single-threaded.
|
||||
|
||||
|
|
|
|||
|
|
@ -57,15 +57,23 @@ first configure needs network access.
|
|||
### Allied Vision Vimba X SDK (proprietary)
|
||||
|
||||
`VmbC`/`VmbCPP` are not in any package manager. Download the Vimba X SDK from Allied Vision
|
||||
(<https://www.alliedvision.com/en/products/software/vimba-x-sdk/>) and point CMake at it:
|
||||
(<https://www.alliedvision.com/en/products/software/vimba-x-sdk/>).
|
||||
|
||||
[cmake/FindVmb.cmake](../cmake/FindVmb.cmake) searches the usual locations — `/opt/VimbaX*`,
|
||||
`/usr/local/VimbaX`, and `~/Programs/VimbaX*` (where the installer unpacks by default on a developer
|
||||
machine) — so an ordinary install needs no flag at all. Point it explicitly only for an unusual path:
|
||||
|
||||
```bash
|
||||
cmake -B build -DVMB_HOME=/opt/VimbaX_2024-1
|
||||
# or: export VIMBA_X_HOME=/opt/VimbaX_2024-1
|
||||
cmake -B build -DVMB_HOME=/opt/VimbaX_2025-3
|
||||
# or: export VIMBA_X_HOME=/opt/VimbaX_2025-3
|
||||
```
|
||||
|
||||
[cmake/FindVmb.cmake](../cmake/FindVmb.cmake) locates the headers/libs and creates `Vmb::VmbC` / `Vmb::VmbCPP`.
|
||||
If the SDK is absent, build with `-DWITH_VIMBA=OFF` to get a mock-only binary.
|
||||
It creates the `Vmb::VmbC` / `Vmb::VmbCPP` imported targets. If the SDK is absent, build with
|
||||
`-DWITH_VIMBA=OFF` to get a mock-only binary.
|
||||
|
||||
**Build with `WITH_VIMBA=ON` before touching `VimbaCameraSource.cpp`.** It is the one source file the
|
||||
mock-only configurations do not compile, so a break in it passes every `-DWITH_VIMBA=OFF` build and
|
||||
only shows up on the rig.
|
||||
|
||||
## Host setup (for a real run)
|
||||
|
||||
|
|
@ -84,8 +92,8 @@ If the SDK is absent, build with `-DWITH_VIMBA=OFF` to get a mock-only binary.
|
|||
## Run
|
||||
|
||||
```bash
|
||||
scripts/run.sh --start # real deployment
|
||||
scripts/run.sh --init --start # find endstops, then start
|
||||
scripts/run.sh --init --start # real deployment: home, then start
|
||||
scripts/run.sh --start # only if already homed this power cycle
|
||||
scripts/run.sh --mock-serial --mock-camera --no-mqtt --start # dev, no hardware
|
||||
```
|
||||
|
||||
|
|
|
|||
|
|
@ -65,8 +65,10 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
|
|||
| `Logging` | `trace` | csv | — | Wire-trace categories, off by default (`--trace` overrides) |
|
||||
| `UI` | `enable_tui` | bool | `false` | Full-screen terminal dashboard (`--tui`/`--no-tui` override; needs `WITH_TUI=ON`) |
|
||||
| `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 |
|
||||
| `Motor` | `yaw_center_count` / `pitch_center_count` | int | `500000` | Count homing **defines** as the middle of the found travel |
|
||||
| `Motor` | `yaw_heading_at_center` | float | `0` | True bearing (0 = north) at that centre (**calibrate**) |
|
||||
| `Motor` | `pitch_angle_at_center` | float | `0` | Elevation above level at that centre (**calibrate**; not 0 on a real rig) |
|
||||
| `Motor` | `yaw_soft_margin_counts` / `pitch_soft_margin_counts` | int | `5000` | Clearance kept from each hard endstop; sent to the firmware, which enforces it |
|
||||
| `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 |
|
||||
|
|
@ -74,47 +76,91 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
|
|||
|
||||
### `[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`), **or** run the
|
||||
automated IMU-referenced `gimbal calib` (below).
|
||||
The firmware reports only **encoder counts**; `[Motor]` maps them to the bearing/elevation degrees
|
||||
used by MQTT (`target_HDG`) and `CamEvent`. **Yaw degrees are true bearings: 0 is north.** Pitch
|
||||
degrees are elevation above level, so 0 is horizontal.
|
||||
|
||||
#### `gimbal calib` — automated IMU-referenced calibration
|
||||
#### The counts are anchored by homing
|
||||
|
||||
`gimbal calib` (requires the IMU) fits each axis's `counts_per_deg` / `zero_count` by sweeping it and
|
||||
correlating encoder counts with the MTi's measured angle. The sequence is deliberately ordered so the
|
||||
**yaw fit is not spoiled by the IMU's heading drift** (see the no-magnetometer discussion below):
|
||||
Homing finds both hard endstops, moves to the middle of that range, and **defines** the midpoint as
|
||||
`<axis>_center_count` (default 500000). Two consequences worth understanding:
|
||||
|
||||
1. **Home if needed** — if the gimbal is not already `READY`, it runs the endstop-finding home first.
|
||||
2. **Pitch at the first yaw position** — yaw moves to the start of its travel and holds there while
|
||||
pitch is swept across its soft-limit travel; at each step it dwells and records the
|
||||
gravity-referenced IMU **pitch** (stable, absolute), then least-squares fits pitch.
|
||||
3. **Pitch → 0°** — using the just-fitted pitch map.
|
||||
4. **Switch to a no-magnetometer XKF profile** — picked from the device's own available-profiles list
|
||||
(a `*nomag*` profile, else a `VRU` one), so the IMU's heading stops chasing the stepper-distorted
|
||||
magnetic field. Sent in the MTi's Config state, so it **persists on the device** across power-cycles.
|
||||
Best-effort: if the device reports no magnetometer-free profile, this step is skipped (logged) and
|
||||
calibration continues.
|
||||
5. **Drift-correct + zero the heading** — holding the gimbal still, it runs the MTi **no-rotation**
|
||||
gyro-bias update (cuts yaw drift), then a **heading reset** so the current pose becomes yaw 0. This
|
||||
happens right before the yaw sweep, so any drift accrued during the slow pitch sweep is discarded.
|
||||
6. **Yaw sweep** — yaw is swept from that first (now zero-heading) position and fitted.
|
||||
- **The origin is reproducible.** Power-cycle the gimbal, re-home, and the limits land on the same
|
||||
numbers. Before this change the count origin was whatever the free-running encoder register
|
||||
happened to hold at boot, so a saved `zero_count` silently pointed somewhere else after a restart.
|
||||
- **Both endstops contribute.** Anchoring on the midpoint averages the error of the two searches
|
||||
instead of inheriting whichever one was used as the reference.
|
||||
|
||||
After calibration the IMU is left on the no-mag profile (this is intentional — the gimbal's homed
|
||||
encoders are the absolute heading reference; the IMU only needs stable roll/pitch and short-term
|
||||
yaw-rate). The `i` view's **XKF profile** row will show the new selection (press `r` to refresh if you
|
||||
changed it outside calibration).
|
||||
A large positive anchor also keeps the whole working range positive, which the driver wants.
|
||||
|
||||
Each axis fit reports an R² (shown in the gimbal `g` view + activity strip). The result is applied to
|
||||
the live session immediately; in the TUI the activity strip then offers to save it to `[Motor]` as the
|
||||
new default (`y`/`n`). `gimbal stop` cancels a run. Timings (dwell, no-rotation duration, etc.) are the
|
||||
`CalibParams` defaults in [CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h).
|
||||
`*_min_deg`/`*_max_deg` are **no longer configured**. They are derived from the firmware's homed soft
|
||||
limits after every home — the only place they can be correct. The host verifies on each home that the
|
||||
reported midpoint really is `<axis>_center_count`; a mismatch means the firmware did not adopt the
|
||||
`CENTER` command (it predates this host) and is reported as an error rather than papered over.
|
||||
|
||||
`*_soft_margin_counts` is the clearance kept from each hard endstop. It is sent to the firmware, which
|
||||
enforces it, and takes effect immediately without re-homing. It is in **counts, not degrees**, because
|
||||
it must apply at homing time — before any degrees↔counts map is trusted — and must stay correct even
|
||||
when the heading calibration is absent or wrong.
|
||||
|
||||
#### `gimbal heading setup` — tying yaw to true north
|
||||
|
||||
Aim at landmarks whose true bearing you know (a mast, a peak, a road junction on the map) and record
|
||||
the encoder position against each bearing. **No IMU is required**, which is the point: not every tower
|
||||
has one, and MTi yaw drifts too much to be a heading reference.
|
||||
|
||||
In the TUI press **`H`** (manual mode only):
|
||||
|
||||
| Key | Action |
|
||||
|---|---|
|
||||
| `←` `→` | Aim yaw |
|
||||
| `↑` `↓` | Aim pitch (centre the landmark vertically) |
|
||||
| `Tab` | Cycle step size: coarse (5% of travel) → fine (0.5%) → very fine (25 counts, ≈0.05°) |
|
||||
| `Space` | Record an image + `CamEvent`, exactly as a scan waypoint would |
|
||||
| `Enter` | Freeze the position and type that landmark's true bearing |
|
||||
| `f` | Fit and apply |
|
||||
| `Esc` | Cancel |
|
||||
|
||||
Two landmarks are the minimum and fit the line exactly; more are least-squares fitted and report an
|
||||
R². **Sightings closer than 10° apart are refused** — two points always fit perfectly, so a short
|
||||
baseline produces a healthy-looking R² and a wildly wrong scale, and refusing is the only way you
|
||||
find out. A set spanning north is handled (bearings are unwrapped before fitting).
|
||||
|
||||
The very-fine step is 25 counts rather than 1 because `ENC_CONST` makes the encoder advance in
|
||||
12.8-microstep quanta: a smaller step moves the motor to a position it cannot reproduce.
|
||||
|
||||
The result is applied to the live session immediately, and in the TUI the activity strip offers to
|
||||
save it to `[Motor]` (`y`/`n`). On a headless tower the same fit is available directly:
|
||||
|
||||
```
|
||||
gimbal heading fit 480000:45.0 520000:115.0
|
||||
gimbal heading offset -2.5 # shift the whole map by a known amount, and save
|
||||
```
|
||||
|
||||
#### `gimbal pitch setup` — levelling pitch against gravity
|
||||
|
||||
`gimbal pitch setup` (**requires the IMU**) fits `pitch_counts_per_deg` and `pitch_angle_at_center` by
|
||||
sweeping pitch and correlating encoder counts with the MTi's gravity-referenced pitch. It auto-homes
|
||||
if needed, dwells at each step, fits, then parks level. Writes `logs/pitch_setup_*.log`, reports R²,
|
||||
and offers the same save prompt. `stop` or Esc cancels. Timings are the `CalibParams` defaults in
|
||||
[PitchSetup.h](../include/fgc/PitchSetup.h).
|
||||
|
||||
This is the one axis that genuinely needs the IMU — level is defined by gravity and nothing else on
|
||||
the tower can measure it. Yaw is untouched by it.
|
||||
|
||||
#### Scan grid
|
||||
|
||||
The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits, always front to
|
||||
back. Set
|
||||
`[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact
|
||||
back. 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.
|
||||
|
||||
**Grid yaw values are true bearings.** Yaw travel is under a full turn, so there is always an
|
||||
unreachable arc; waypoints falling in it are **skipped** with a warning rather than clamped to the
|
||||
nearest limit — clamping would image a direction nobody asked for and publish it under the bearing
|
||||
they did. A one-line audit after each home reports how many waypoints are affected. Generated grids
|
||||
interpolate `yaw_min_deg → yaw_max_deg` linearly and wrap each point, so an arc crossing north is
|
||||
written monotonically (`340` → `380`).
|
||||
|
||||
Each camera's output subfolder is its `[Camera.<name>] label` (defaulting to the block name); nothing is derived from the order of the blocks.
|
||||
|
||||
### `[IMU]` — Xsens MTi orientation sensor
|
||||
|
|
@ -268,8 +314,9 @@ section, it comes from `config.ini` rather than from the device.
|
|||
**To check it on the rig:** level the gimbal — roll and pitch should read ≈0. Tip the nose up — pitch
|
||||
goes positive. Roll to the right — roll goes positive. Slew right — yaw increases.
|
||||
|
||||
**Re-run `gimbal calib` after changing the mount.** Calibration fits IMU yaw against encoder counts, so
|
||||
a calibration made under a different (or absent) mount has that error baked into `yaw_zero_count`.
|
||||
**Re-run `gimbal pitch setup` after changing the mount.** It fits IMU pitch against encoder counts, so
|
||||
a calibration made under a different (or absent) mount has that error baked into
|
||||
`pitch_angle_at_center`. Yaw is unaffected — `gimbal heading setup` uses landmarks, not the IMU.
|
||||
|
||||
`imu zero` is unaffected: it acts on the device, before this rotation, and the two compose correctly —
|
||||
the mount is what makes "zero" mean zero *in the gimbal frame*.
|
||||
|
|
@ -452,7 +499,7 @@ one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
|
|||
|
||||
- **Gimbal** (`g`) — both axes side by side: live telemetry, the decoded firmware register **dump**
|
||||
(`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis
|
||||
`counts_per_deg` / `zero_count` / R² / age).
|
||||
`counts_per_deg` / `deg@ctr` / R² / age).
|
||||
- **Cameras** (`c`) — a block per configured camera: identity (`name`, `label`, `id`, `driver`,
|
||||
enabled), the active **profile** and the others available, the `profile_dir` and the exact XML
|
||||
applied, the per-profile `settle_wait_ms`, `rotation_deg` and the JPEG XL settings, plus live
|
||||
|
|
@ -472,18 +519,21 @@ one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
|
|||
(`enable_env` / `--mock-env`) or, when enabled but silent, the staleness window that was missed.
|
||||
|
||||
**Activity strip** — a compact section between the log and the key bar that shows the
|
||||
currently-running special operation with live progress (`test`, `gimbal calib`, homing, capture
|
||||
currently-running special operation with live progress (`test`, `gimbal pitch setup`, homing, capture
|
||||
scan) **and persists the last test/calibration result** (PASS/FAIL, age) so it doesn't scroll
|
||||
away in the log. It only appears once something has run. While a procedure is running, **Esc**
|
||||
cancels it.
|
||||
|
||||
When a `gimbal calib` completes it is applied to the live session and the strip shows a highlighted
|
||||
yes/no prompt — **`Save this calibration to config as the new default? (y / n)`**. Press **`y`** to
|
||||
write the full `[Motor]` per-axis map — `*_counts_per_deg`, `*_zero_count`, and `*_min_deg`/`*_max_deg`
|
||||
— back into the `config.ini` the program was launched with (replacing those keys in place, preserving
|
||||
everything else) so it persists across restarts; press **`n`** to keep it for this session only. The keys are active only while the prompt is
|
||||
showing. (Until you answer `y`, calibration remains session-only — see
|
||||
[known-issues.md](known-issues.md).)
|
||||
When a `gimbal pitch setup` or `gimbal heading fit` completes it is applied to the live session and the
|
||||
strip shows a highlighted yes/no prompt — **`Save the new [Motor] calibration to config as the
|
||||
default? (y / n)`**. Press **`y`** to write the `[Motor]` map — `*_counts_per_deg`, `*_center_count`,
|
||||
`yaw_heading_at_center`/`pitch_angle_at_center` and `*_soft_margin_counts` — back into the
|
||||
`config.ini` the program was launched with (replacing those keys in place, preserving comments and
|
||||
everything else) so it persists across restarts; press **`n`** to keep it for this session only. The
|
||||
keys are active only while the prompt is showing. The file is written to a temp file and renamed over
|
||||
the original, so an interrupted save cannot truncate your config.
|
||||
|
||||
`gimbal heading offset` saves immediately without asking — it is a one-line correction, not a fit.
|
||||
|
||||
**The key bar is contextual**: it advertises the keys that are useful in the current view, not every
|
||||
binding. `s` start and `x` stop lead everywhere — a scan can be started and stopped from any view —
|
||||
|
|
|
|||
|
|
@ -137,7 +137,7 @@ frames, and confirm discovery with the Vimba X `VimbaXViewer`/`ListCameras` tool
|
|||
```bash
|
||||
cd /opt/fire_gimbal_control
|
||||
set -a; . /etc/fire_gimbal_control/credentials.env; set +a
|
||||
./fire_gimbal_control --config ./config.ini --start --log-level debug
|
||||
./fire_gimbal_control --config ./config.ini --init --start --log-level debug
|
||||
```
|
||||
|
||||
Watch for: `Loaded config`, `Serial controller started`, `Opened camera ...`, `MQTT connected`, then
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ doctest unit-test suite (`ctest`).
|
|||
|
||||
| # | Issue | Status |
|
||||
|---|-------|--------|
|
||||
| 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. |
|
||||
| 13 | `[Motor]` degrees↔counts calibration | The `config.example.ini` values are **placeholders**. Home, then run `gimbal heading setup` (yaw, needs no IMU) and `gimbal pitch setup` (pitch, needs the IMU) on the rig. |
|
||||
| 14 | Capture sweep on hardware | **Mechanically verified on the rig (2026-08-07).** A full `--init --start` run homed both axes and completed `MOVE → settle → trigger` for 15/15 waypoints with 0 dropped frames, 0 stalls and 0 acquisition failures. `kSettleTolCounts` (600) and the per-interval timing in [CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) have **not** been tuned against observed `ST` behaviour yet, and the settle tolerance has not been checked against a sharp image (see #16). |
|
||||
| 15 | **Camera profiles unverified on a lit scene** | The `standard`/`night` profiles must be exported from the VimbaX Viewer (see `config/camera_profiles/rgb/README.md`) and have not been measured against real light. Confirm on the rig that `LoadSettings` succeeds, that `standard` sustains 10 fps with `frames_dropped` at 0, and that the auto-exposure lands on a sensible `ExposureTime` outdoors. The only previous rig test ran with the lens covered. |
|
||||
| 16 | **`settle_wait_ms` untuned** | `standard` waits 1 s and `night` 2.5 s after standstill before keeping a frame. Both are reasoned (≫ one frame period; long enough for the auto-exposure to see the new scene) but not measured. Check on a lit scene that the kept frame shows no slew blur and that its exposure has actually converged for that heading, and shorten the wait if it has. |
|
||||
|
|
@ -124,17 +124,20 @@ scripts for this are worth keeping; note that wrapping them in `timeout` SIGTERM
|
|||
- **`[IMU] x` should be re-confirmed now that the reference is corrected.** `x = 180` was
|
||||
set while pitch was inverted. Level the gimbal and confirm roll and pitch ≈ 0, tip the
|
||||
nose up and confirm pitch goes **positive**, roll right and confirm roll goes positive.
|
||||
Then re-run `gimbal calib` — its yaw reference moved with the earth-frame correction.
|
||||
Then re-run `gimbal pitch setup` — its reference moved with the earth-frame correction.
|
||||
Yaw is unaffected: `gimbal heading setup` uses landmarks, not the IMU.
|
||||
- **The handshake still does not await acks for its state transitions.**
|
||||
`GoToConfig`/`GoToMeasurement` are blind writes separated by 60 ms sleeps
|
||||
([MtiImuSource.cpp](../src/serial/MtiImuSource.cpp)). The readback verification makes
|
||||
this harmless for correctness — if `GoToConfig` is missed the Sets do nothing and the
|
||||
mismatch is reported — but a missed transition is diagnosed one step later than it
|
||||
happened.
|
||||
- **A saved calibration silently predates the IMU mount.** `gimbal calib` fits IMU yaw against encoder
|
||||
counts, so a calibration made before `[IMU] x/y/z` were set — or under different values — has that
|
||||
mount error baked into `yaw_zero_count`. Nothing detects this; re-run `gimbal calib` after changing
|
||||
the mount. Stamping the mount into the saved `[Motor]` block and warning on a mismatch would close it.
|
||||
- **A saved pitch calibration silently predates the IMU mount.** `gimbal pitch setup` fits IMU pitch
|
||||
against encoder counts, so a calibration made before `[IMU] x/y/z` were set — or under different
|
||||
values — has that mount error baked into `pitch_angle_at_center`. Nothing detects this; re-run
|
||||
`gimbal pitch setup` after changing the mount. Stamping the mount into the saved `[Motor]` block and
|
||||
warning on a mismatch would close it. Yaw no longer has this exposure at all, since
|
||||
`gimbal heading setup` never consults the IMU.
|
||||
- **The mount is host-side only.** The rotation is applied to the MTi's output, so the XKF filter itself
|
||||
still runs in the sensor frame. Xsens `SetAlignmentRotation` (MID 0xEC) would push the alignment into
|
||||
the device so the filter's own gravity/magnetic corrections are made in the gimbal frame — better
|
||||
|
|
@ -153,18 +156,17 @@ scripts for this are worth keeping; note that wrapping them in `timeout` SIGTERM
|
|||
thermal or NIR unit needs a new `ICameraSource` implementation behind that key.
|
||||
- Reintroduce optional image upload to the ground station, config-driven (the old hardcoded NFS/SMB upload was
|
||||
removed).
|
||||
- **`gimbal calib` persistence**: the fitted `counts_per_deg`/`zero_count` are always applied to the live
|
||||
session (display, manual moves, MQTT heading, **and** the capture scheduler) and written to
|
||||
`logs/calib_*.log`. In the **TUI**, the activity strip then prompts `Save … as the new default? (y/n)`
|
||||
— `y` writes the full per-axis `[Motor]` map (`*_counts_per_deg`, `*_zero_count`,
|
||||
`*_min_deg`/`*_max_deg`) into `config.ini` (persists across restarts), `n` keeps them
|
||||
session-only. In the **headless** console there is no prompt, so calibration stays session-only there;
|
||||
copy the logged values into `[Motor]` by hand to keep them.
|
||||
- **Calibration persistence needs the TUI.** `gimbal pitch setup` and `gimbal heading fit` are always
|
||||
applied to the live session (display, manual moves, MQTT bearings, **and** the capture scheduler) and
|
||||
logged. In the **TUI**, the activity strip then prompts `Save … as the default? (y/n)` — `y` writes
|
||||
the `[Motor]` map into `config.ini`, `n` keeps it session-only. In the **headless** console there is
|
||||
no prompt, so it stays session-only there; copy the logged values into `[Motor]` by hand.
|
||||
`gimbal heading offset` is the exception — it saves immediately, with no prompt.
|
||||
- **`gimbal nudge`** moves a fixed fraction of the **homed** endstop-to-endstop travel (from the
|
||||
firmware dump), so it is independent of the degrees↔counts calibration. If no dump has been captured
|
||||
yet (not homed / no `gimbal dump`), nudge requests one and does nothing that press — it never falls
|
||||
back to the configured degree clamps (which, if `*_min_deg`/`*_max_deg` were unset, produced absurd
|
||||
±100000° steps).
|
||||
back to a degree-derived guess. `gimbal jog` is the raw-counts equivalent when a finer step is
|
||||
wanted.
|
||||
- Ambient temperature/humidity (SHT41, `IEnvSensor`/`Sht41EnvSensor`/`MockEnvSensor`) is fully
|
||||
integrated and **validated against the physical sensor on the LattePanda** — config-gated
|
||||
(`[Features] enable_env`, `[Env]`), Sensors panel + expanded `i` view + MQTT `Env` topic wired.
|
||||
|
|
|
|||
|
|
@ -17,7 +17,8 @@ Per-file reference for the refactored tree, plus the shared data structures.
|
|||
| [include/fgc/DumpParser.h](../include/fgc/DumpParser.h), [src/core/DumpParser.cpp](../src/core/DumpParser.cpp) | `parseDump` firmware `DUMP` block → `DumpData` (per-axis state + decoded TMC registers/flags); `formatDump` |
|
||||
| [include/fgc/DiagParser.h](../include/fgc/DiagParser.h), [src/core/DiagParser.cpp](../src/core/DiagParser.cpp) | `parseDiag` firmware `DG` self-test stream → `DiagResult` (per-axis tests, PASS/FAIL); `formatDiag` |
|
||||
| [include/fgc/Calibration.h](../include/fgc/Calibration.h), [src/core/Calibration.cpp](../src/core/Calibration.cpp) | `linearFit` (least-squares, R²) + `circularMeanDeg` for the IMU-referenced calibration |
|
||||
| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: home-if-needed → pitch sweep at the first yaw position → pitch 0° → switch IMU to a no-mag XKF profile → IMU no-rotation + heading reset → yaw sweep, fitting degrees↔counts; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()` |
|
||||
| [include/fgc/PitchSetup.h](../include/fgc/PitchSetup.h), [src/core/PitchSetup.cpp](../src/core/PitchSetup.cpp) | `gimbal pitch setup` worker thread: home-if-needed → pitch sweep dwelling on the gravity-referenced IMU pitch → least-squares fit → park level; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()`. Yaw is not touched |
|
||||
| [include/fgc/HeadingFit.h](../include/fgc/HeadingFit.h), [src/core/HeadingFit.cpp](../src/core/HeadingFit.cpp) | `gimbal heading fit`: least-squares fit of the yaw map from operator sightings of landmarks with known true bearings. Sorts by encoder position, unwraps bearings so a set spanning north fits one line, and refuses a baseline under 10° (two points always fit exactly, so a short baseline looks perfect and is wrong). No IMU involved |
|
||||
| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**); orientation-control builders `msgSetNoRotation` (gyro-bias update) + `msgResetOrientation` (heading reset / store) + `msgSetFilterProfile` (select XKF profile) + `pickNoMagProfile` (choose a magnetometer-free profile from the available list); named `mtmode`/`mtsettings` bitfields with `mtDataLen` deriving the payload length from them (so `kMTDataLen` cannot drift from what the parser decodes), plus `checkImuDeviceConfig` → `ImuConfigMismatch` (does the device report what the host asked for?) and `mtiErrorText` for MID `0x42` rejections |
|
||||
| [include/fgc/ImuMount.h](../include/fgc/ImuMount.h), [src/core/ImuMount.cpp](../src/core/ImuMount.cpp) | `ImuMount` (the `[IMU] x/y/z` mounting rotation) and the pure math around it: `mountRotationZXY` (intrinsic Z-X-Y), the `rotationFromEulerZYX`/`eulerZYXFromRotation` pair for the MTi's own yaw-pitch-roll output, `applyFrames`, which moves a whole `ImuSample` into the gimbal's NED frame by re-referencing the earth frame (`ImuReference`/`nedFromReference`, left-multiplied) and then applying the mount (right-multiplied) — the two are not interchangeable, which is why a mount alone cannot correct a Z-up device; `applyMount` is the NED-already shorthand. Body-frame vectors are rotated by the mount only. Returns the sample untouched when there is nothing to do |
|
||||
| [include/fgc/MountedImuSource.h](../include/fgc/MountedImuSource.h) | Decorator over any `IImuSource` applying that rotation in `sample()`. Wrapped around the factory's output in `Application::makeImu()`, so every consumer reads gimbal-frame values without knowing a mount exists; skipped entirely when the mount is the identity |
|
||||
|
|
@ -111,7 +112,7 @@ Owned pixel buffer + `width`, `height`, `channels` (1 or 3), `timestamp_ms`, `ca
|
|||
One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity), `gyr[3]` (rad/s),
|
||||
`mag[3]` (a.u., earth-normalized), `roll_deg` (−180..180), `pitch_deg` (−90..90), `yaw_deg`
|
||||
(**−180..180**, exactly as the device reports it), `sample_counter`. `parseMTData` applies no sign flip and no re-ranging to any axis.
|
||||
`gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean
|
||||
`gimbal heading fit` phase-unwraps the sighted bearings (`unwrapNear`) so a set crossing 0/360 still fits a clean
|
||||
line.
|
||||
|
||||
### `ImuDeviceConfig` ([MtiProtocol.h](../include/fgc/MtiProtocol.h))
|
||||
|
|
|
|||
|
|
@ -33,7 +33,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 yaw target used in ControlCode 1 (converted to encoder counts via `[Motor]`) |
|
||||
| `GGS/FWT/<tower>/target_HDG` | integer bearing as string, e.g. `"137"` | validated with `stoi`, stored as **string** | Sets the yaw target used in ControlCode 1 (a **true bearing**, 0 = north, 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.
|
||||
|
|
@ -48,6 +48,11 @@ flags** so each update is acted on once (`MqttControlChannel::poll()`).
|
|||
| `0` | **Automatic sweep.** Walk the scan grid front to back, then start over: `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. |
|
||||
|
||||
> **`target_HDG` is a true bearing and can be refused.** Yaw travel is under a full turn, so there is
|
||||
> always an unreachable arc. A bearing falling in it is logged once and the gimbal **holds position**;
|
||||
> it is not clamped to the nearest limit, which would image a direction up to 180° away and publish it
|
||||
> under the bearing that was asked for. The reachable arc is logged after every home.
|
||||
|
||||
When a ControlCode message arrives, the program echoes the current code back on the StatusCode topic.
|
||||
|
||||
## Published topics (outbound)
|
||||
|
|
@ -69,7 +74,7 @@ Built by `MqttControlChannel::publishCamEvent` from a `CamEvent`:
|
|||
|-------|------|---------|
|
||||
| `fwt` | string | Tower name (`config.ini` `tower_name`) |
|
||||
| `cam` | string | Camera label: `RGB`, `ACR`, or `NIR` (by camera index) |
|
||||
| `hdg` | int | Gimbal yaw heading **× 10** (one decimal place encoded as integer) at capture time, from the **yaw encoder** |
|
||||
| `hdg` | int | Gimbal **true bearing** × 10 (0 = north) at capture time, from the **yaw encoder** |
|
||||
| `pit` | int | Gimbal pitch elevation **× 10** at capture time, from the **IMU** — elevation relative to gravity, i.e. true pointing angle |
|
||||
| `time` | int | Capture timestamp, Unix epoch **milliseconds** (matches the `.jxl` filename) |
|
||||
|
||||
|
|
@ -81,7 +86,8 @@ different sources deliberately. The pitch encoder only measures the axis relativ
|
|||
gimbal's own base, so a leaning mast is invisible to it; the IMU measures against gravity
|
||||
and therefore reports the elevation the imagery is actually pointing at. Yaw stays on the
|
||||
encoder because IMU yaw drifts and depends on the heading reset and XKF profile, whereas
|
||||
encoder yaw is calibrated against the IMU by `gimbal calib` and then holds.
|
||||
encoder yaw is tied to true north by `gimbal heading setup` (sighting known
|
||||
landmarks, no IMU involved) and then holds.
|
||||
|
||||
Two consequences for consumers:
|
||||
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ firmware-only `gimbal diag` (the firmware motor self-test is now one leaf inside
|
|||
`test gimbal encoder`).
|
||||
|
||||
Implementation: [`TestRunner`](../src/core/TestRunner.cpp) (worker thread, mirrors
|
||||
`CalibrationRoutine`) + the pure [`TestReport`](../src/core/TestReport.cpp) model
|
||||
`PitchSetupRoutine`) + the pure [`TestReport`](../src/core/TestReport.cpp) model
|
||||
(`formatTestReport`/`parseTestReport`/`applyComparison`) and
|
||||
[`HostMetrics`](../src/core/HostMetrics.cpp) host reads.
|
||||
|
||||
|
|
|
|||
|
|
@ -5,8 +5,9 @@
|
|||
|
||||
namespace fgc {
|
||||
|
||||
// Pure math helpers for `gimbal calib`. Kept I/O-free in fgc_core so the fit can
|
||||
// be unit-tested independently of the threaded routine that drives the hardware.
|
||||
// Pure math helpers shared by `gimbal pitch setup` and `gimbal heading fit`.
|
||||
// Kept I/O-free in fgc_core so the fits can be unit-tested independently of the
|
||||
// threaded routine that drives the hardware.
|
||||
|
||||
struct LinearFit {
|
||||
bool ok = false; // false if < 2 points or x has no spread
|
||||
|
|
@ -17,7 +18,8 @@ struct LinearFit {
|
|||
};
|
||||
|
||||
// Least-squares fit of y over x. For calibration: x = measured degrees,
|
||||
// y = motor encoder counts ⇒ slope = counts_per_deg, intercept = zero_count.
|
||||
// y = motor encoder counts ⇒ slope = counts_per_deg, and the intercept gives the
|
||||
// angle at the anchored centre (see AxisMap).
|
||||
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy);
|
||||
|
||||
// Circular mean of angles in degrees, robust to ±180° wrap (e.g. IMU heading).
|
||||
|
|
|
|||
|
|
@ -39,6 +39,13 @@ public:
|
|||
void setCaptureActive(bool active);
|
||||
bool captureActive() const { return capture_active_; }
|
||||
|
||||
// Take one image now, outside any scan (the TUI's spacebar / `snapshot`).
|
||||
// Serviced on the next tick once the gimbal is standing still and the settle
|
||||
// wait has elapsed, so the frame is as clean as a scan frame. Ignored while a
|
||||
// capture is active. Idempotent: asking twice before it fires yields one image.
|
||||
void requestSnapshot();
|
||||
bool snapshotPending() const { return snapshot_pending_; }
|
||||
|
||||
void setImageRate(double rate); // images per second
|
||||
double imageRate() const { return image_rate_; }
|
||||
|
||||
|
|
@ -48,7 +55,7 @@ public:
|
|||
void setSettleWait(int settle_ms, int timeout_ms);
|
||||
int settleWaitMs() const { return settle_wait_ms_; }
|
||||
|
||||
// Replace the degrees<->counts maps (e.g. after `gimbal calib`) so subsequent
|
||||
// Replace the degrees<->counts maps (e.g. after `gimbal heading fit`) so subsequent
|
||||
// scan waypoints target with the calibrated conversion. Called on the control
|
||||
// thread, same as tick(), so no locking is needed.
|
||||
void setGeometry(const Geometry& g) { geometry_ = g; }
|
||||
|
|
@ -68,6 +75,7 @@ private:
|
|||
void resetTimer();
|
||||
void sendMove(); // emit MOVE to (yaw_target_, pitch_target_)
|
||||
bool settledAt(const MotorTelemetry& t) const;
|
||||
void serviceSnapshot(const MotorTelemetry& t);
|
||||
|
||||
IMotorController& motor_;
|
||||
ICameraSource& camera_;
|
||||
|
|
@ -88,6 +96,13 @@ private:
|
|||
int settle_wait_ms_ = 1000;
|
||||
int frame_timeout_ms_ = 2000;
|
||||
bool pitch_present_ = false; // a P: segment was seen in telemetry (2-axis rig)
|
||||
// One warning per run for each unreachable-heading case: a grid with a few
|
||||
// out-of-arc waypoints would otherwise log on every pass, forever.
|
||||
bool unreachable_warned_ = false;
|
||||
bool directed_unreachable_warned_ = false;
|
||||
|
||||
bool snapshot_pending_ = false; // a manual snapshot is armed
|
||||
long long snapshot_settled_ms_ = 0; // when standstill was first seen (0 = not yet)
|
||||
long yaw_target_ = 0; // current target, encoder counts
|
||||
long pitch_target_ = 0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -171,11 +171,6 @@ struct TestConfig {
|
|||
const TestProfile* find(const std::string& name) const; // nullptr if absent
|
||||
};
|
||||
|
||||
// Which homed endstop becomes yaw 0 deg, with degrees rising toward the other
|
||||
// limit (which then lands near +360, a bit less for the soft-limit/mechanical
|
||||
// gap). Off = use the configured/calibrated yaw zero_count as-is.
|
||||
enum class YawHomeZero { Off, Low, High };
|
||||
|
||||
struct AppConfig {
|
||||
GeneralConfig general;
|
||||
NetworkConfig network;
|
||||
|
|
@ -186,7 +181,11 @@ struct AppConfig {
|
|||
LoggingConfig logging;
|
||||
UiConfig ui; // [UI] terminal dashboard toggle
|
||||
Geometry geometry; // [Motor] degrees<->counts maps (yaw + pitch)
|
||||
YawHomeZero yaw_home_zero = YawHomeZero::Off; // [Motor] re-anchor yaw zero on homing
|
||||
// [Motor] clearance kept from each hard endstop, in counts. Sent to the
|
||||
// firmware (MARGIN), which enforces it; kept in counts rather than degrees
|
||||
// because it must apply at homing time, before any degree map is trusted.
|
||||
long yaw_soft_margin_counts = 5000;
|
||||
long pitch_soft_margin_counts = 5000;
|
||||
long enc_error_warn_counts = 400; // [Motor] live encoder-error WARN (0=off)
|
||||
ScanConfig scan; // [Scan] grid source
|
||||
ImuConfig imu; // [IMU] Xsens MTi serial device
|
||||
|
|
@ -220,8 +219,11 @@ public:
|
|||
std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
|
||||
const std::vector<std::pair<std::string, std::string>>& kv);
|
||||
|
||||
// Persist the live `[Motor]` calibration (counts_per_deg / zero_count for both
|
||||
// axes) back into the INI file at `path`. Returns true on success.
|
||||
bool saveMotorCalibration(const std::string& path, const Geometry& geo);
|
||||
// Persist the live `[Motor]` geometry (counts_per_deg / center_count /
|
||||
// deg_at_center for both axes, plus the endstop margins) back into the INI file
|
||||
// at `path`. Written via a temp file and rename, so an interrupted save cannot
|
||||
// leave a truncated config behind. Returns true on success.
|
||||
bool saveMotorGeometry(const std::string& path, const Geometry& geo, long yaw_margin,
|
||||
long pitch_margin);
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -14,11 +14,11 @@ struct DumpAxis {
|
|||
char axis = '?'; // 'Y' (yaw) / 'P' (pitch)
|
||||
std::string state_name = "?"; // BOOT/RESET/HOMING/READY/ERROR
|
||||
bool enabled = false;
|
||||
bool eeprom_restored = false;
|
||||
bool has_encoder = false;
|
||||
long lim_neg = 0;
|
||||
long lim_pos = 0;
|
||||
long soft_margin = 0; // counts kept clear of each hard limit
|
||||
long center_count = 0; // count the firmware anchored the homed midpoint to
|
||||
long hold_target = 0;
|
||||
long speed = 0;
|
||||
// Encoder-tracking health accumulated by the firmware hold corrector.
|
||||
|
|
|
|||
|
|
@ -1,27 +1,53 @@
|
|||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Affine map between heading degrees and absolute encoder counts for one axis:
|
||||
// Normalise an angle to [0, 360) / [-180, 180). Free functions because headings
|
||||
// are compared and differenced in several places outside AxisMap.
|
||||
double wrap360(double deg);
|
||||
double wrap180(double deg);
|
||||
|
||||
// Affine map between degrees and absolute encoder counts for one axis, anchored
|
||||
// on the *mechanical centre* found by homing:
|
||||
//
|
||||
// counts = zero_count + deg * counts_per_deg
|
||||
// deg = (counts - zero_count) / counts_per_deg
|
||||
// counts = center_count + (deg - deg_at_center) * counts_per_deg
|
||||
// deg = deg_at_center + (counts - center_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).
|
||||
// The firmware speaks only in encoder counts. `center_count` is the count the
|
||||
// firmware assigns to the midpoint of the homed travel (it is told this value,
|
||||
// see the CENTER command), so it is reproducible across power cycles - unlike the
|
||||
// free-running encoder origin it replaced. `deg_at_center` is what that midpoint
|
||||
// points at: a true compass heading for yaw (0 = north), an elevation above level
|
||||
// for pitch (+ = up).
|
||||
//
|
||||
// min_deg/max_deg are DERIVED from the homed soft limits after each home, not
|
||||
// configured - the firmware's limits are the only place they can be correct.
|
||||
// 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
|
||||
long center_count = 500000; // count at the homed mechanical centre
|
||||
double deg_at_center = 0.0; // heading/elevation there
|
||||
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).
|
||||
// Counts -> degrees. Returns deg_at_center if counts_per_deg is ~0
|
||||
// (misconfigured), i.e. it degrades to "we are at the centre" rather than to
|
||||
// an arbitrary zero.
|
||||
double toDeg(long counts) const;
|
||||
|
||||
// Yaw only: the same reading as a compass heading in [0, 360).
|
||||
double headingDeg(long counts) const;
|
||||
|
||||
// Yaw only: counts for a true heading, taking the short way round. Returns
|
||||
// nullopt when the heading lies in the arc the gimbal cannot reach - the
|
||||
// travel is under 360 deg, so there is always a dead sector, and pointing at
|
||||
// the nearest limit instead would silently image the wrong direction.
|
||||
std::optional<long> headingToCounts(double heading_deg) const;
|
||||
};
|
||||
|
||||
// Per-axis maps for the two-axis gimbal.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,48 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Fitting the yaw map from operator sightings of known landmarks.
|
||||
//
|
||||
// The gimbal is aimed at something whose true bearing is known (a mast, a peak, a
|
||||
// road junction on the map), and the encoder position is recorded against that
|
||||
// bearing. Two sightings determine the line exactly; more are least-squares
|
||||
// fitted. Nothing here needs an IMU, which is the point: not every tower has one,
|
||||
// and IMU yaw drifts badly enough that it was never a trustworthy heading source.
|
||||
//
|
||||
// Kept I/O-free in fgc_core so the fit is unit-testable without hardware.
|
||||
|
||||
struct HeadingSighting {
|
||||
long counts = 0; // yaw encoder position when the landmark was centred
|
||||
double heading_deg = 0.0; // its true bearing, degrees (0 = north)
|
||||
};
|
||||
|
||||
struct HeadingFitResult {
|
||||
bool ok = false;
|
||||
std::string error; // operator-facing reason when !ok
|
||||
double counts_per_deg = 0.0;
|
||||
double deg_at_center = 0.0; // bearing at center_count, normalised to [0,360)
|
||||
double r2 = 0.0;
|
||||
int n = 0;
|
||||
double span_deg = 0.0; // angular baseline actually used
|
||||
};
|
||||
|
||||
// Shortest baseline we will fit. Two sightings a few degrees apart put the whole
|
||||
// scale on a handful of counts, so a small aiming error swings counts_per_deg
|
||||
// wildly - and the result looks perfectly healthy (two points always fit exactly,
|
||||
// R^2 = 1). Refusing is the only way the operator finds out.
|
||||
inline constexpr double kMinHeadingBaselineDeg = 10.0;
|
||||
|
||||
// Fit counts_per_deg and the bearing at `center_count`. Sightings may be given in
|
||||
// any order and may span north.
|
||||
HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count);
|
||||
|
||||
// Parse "<counts>:<deg>" tokens (the form the TUI wizard emits). Returns false and
|
||||
// sets `error` on the first malformed token.
|
||||
bool parseHeadingSightings(const std::vector<std::string>& tokens,
|
||||
std::vector<HeadingSighting>& out, std::string& error);
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -72,7 +72,10 @@ public:
|
|||
// which camera on every keystroke would make that worse, not better.
|
||||
void setDistance(double d) { for (auto& c : params_.cameras) c.jxl_distance = d; }
|
||||
void setEffort(int e) { for (auto& c : params_.cameras) c.jxl_effort = e; }
|
||||
void setDisplay(bool on) { params_.display = on; }
|
||||
// Toggled from the control thread while the worker reads it, hence atomic.
|
||||
// The worker also CLEARS it when the display turns out to be unusable, so this
|
||||
// is genuinely shared state rather than a one-way setting.
|
||||
void setDisplay(bool on) { display_.store(on); }
|
||||
|
||||
// Last CamEvent published (camera label, heading/pitch, timestamp), for the
|
||||
// UI's camera panel. Thread-safe; nullopt until the first capture is saved.
|
||||
|
|
@ -105,6 +108,10 @@ private:
|
|||
std::atomic<bool> running_{false};
|
||||
std::thread worker_;
|
||||
|
||||
// Live preview state. Seeded from Params::display, then owned by setDisplay()
|
||||
// and by process()'s self-disable on a display failure.
|
||||
std::atomic<bool> display_{false};
|
||||
|
||||
mutable std::mutex last_event_mutex_;
|
||||
std::optional<CamEvent> last_event_;
|
||||
std::atomic<long long> saved_{0}; // images successfully written this session
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ namespace fgc {
|
|||
class IMotorController;
|
||||
class IImuSource;
|
||||
|
||||
// Live progress of a running calibration, for the TUI activity strip.
|
||||
// Live progress of a running pitch setup, for the TUI activity strip.
|
||||
struct CalibProgress {
|
||||
bool running = false;
|
||||
char axis = '?'; // 'Y' / 'P'
|
||||
|
|
@ -23,13 +23,13 @@ struct CalibProgress {
|
|||
std::string phase; // "moving" / "dwelling" / "fitting" / ...
|
||||
};
|
||||
|
||||
// Structured outcome of the last completed calibration (persists for display).
|
||||
// Structured outcome of the last completed pitch setup (persists for display).
|
||||
struct CalibReport {
|
||||
struct Axis {
|
||||
char axis = '?';
|
||||
bool ok = false;
|
||||
double counts_per_deg = 0;
|
||||
long zero_count = 0;
|
||||
double deg_at_center = 0;
|
||||
double r2 = 0;
|
||||
int n = 0;
|
||||
};
|
||||
|
|
@ -42,36 +42,37 @@ struct CalibReport {
|
|||
// Tunable timings/sizes for a calibration run. Defaults are the production
|
||||
// values; tests inject tiny ones so a full run completes in milliseconds.
|
||||
struct CalibParams {
|
||||
int positions = 10; // sweep steps per axis
|
||||
int positions = 10; // sweep steps
|
||||
int dwell_ms = 5000; // hold at each position while sampling
|
||||
int sample_ms = 100; // IMU sampling cadence during a dwell
|
||||
int settle_timeout_ms = 15000; // max wait for a single MOVE to settle
|
||||
int home_timeout_ms = 90000; // max wait for HOME to reach READY
|
||||
int norotation_s = 3; // no-rotation gyro-bias update duration
|
||||
int reset_settle_ms = 1500; // let the filter apply the heading reset
|
||||
double inset_frac = 0.05; // keep targets off the hard endstops
|
||||
};
|
||||
|
||||
// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
|
||||
// thread (the motor/imu/Logger interfaces are thread-safe). Sequence:
|
||||
// 1. home the gimbal if it is not already READY;
|
||||
// 2. move yaw to its first sweep position and calibrate PITCH there (sweep its
|
||||
// soft-limit travel, dwelling to record the gravity-referenced IMU pitch);
|
||||
// 3. move pitch to 0 deg;
|
||||
// 4. switch the IMU to a no-magnetometer XKF profile (persists on the device) so
|
||||
// its heading stops chasing the stepper-distorted magnetic field;
|
||||
// 5. with the gimbal held still, run the IMU no-rotation update (cuts yaw drift)
|
||||
// then reset the IMU heading so the current pose is yaw 0 — this removes the
|
||||
// drift accumulated during the slow pitch sweep right before it matters;
|
||||
// 6. sweep YAW from that first position and calibrate it.
|
||||
// Each axis fit is least-squares (counts vs degrees). The resulting Geometry is
|
||||
// published via takeResult() for the main thread to apply; raw samples + fits are
|
||||
// written to a logfile. Progress streams to the LOG pane. Cancellable, one-at-a-time.
|
||||
class CalibrationRoutine {
|
||||
// `gimbal pitch setup`: an IMU-referenced steps<->degrees calibration for the
|
||||
// PITCH axis only. Runs on its own thread (the motor/imu/Logger interfaces are
|
||||
// thread-safe). Sequence:
|
||||
// 1. abort unless the IMU is connected - there is no other way to find level;
|
||||
// 2. home the gimbal if it is not already READY;
|
||||
// 3. sweep pitch across its soft-limit travel, dwelling at each step to record
|
||||
// the gravity-referenced IMU pitch, and least-squares fit counts vs degrees;
|
||||
// 4. park at 0 deg (level) using the map just fitted.
|
||||
//
|
||||
// Yaw is deliberately absent. It is tied to true north by `gimbal heading setup`
|
||||
// against landmarks the operator can identify, which needs no IMU (not every
|
||||
// tower has one) and does not inherit the MTi's heading drift - the reason the
|
||||
// old two-axis routine had to switch XKF profiles and reset the heading mid-run
|
||||
// just to get one usable yaw sweep.
|
||||
//
|
||||
// The resulting Geometry is published via takeResult() for the main thread to
|
||||
// apply; raw samples + the fit are written to a logfile. Progress streams to the
|
||||
// LOG pane. Cancellable, one-at-a-time.
|
||||
class PitchSetupRoutine {
|
||||
public:
|
||||
CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
||||
PitchSetupRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
||||
CalibParams params = {});
|
||||
~CalibrationRoutine();
|
||||
~PitchSetupRoutine();
|
||||
|
||||
// Begin on a worker thread. Logs a reason and returns false if already
|
||||
// running (prechecks happen on the worker and abort there).
|
||||
|
|
@ -12,8 +12,8 @@ namespace fgc {
|
|||
// ST Y:<state>,<xactual>,<xenc>,<drv_hex>,<sg>,<cs>,<pwm>,<flags> [P:...]
|
||||
//
|
||||
// where <state> is B/R/H/A/E and <flags> is an optional run of status letters
|
||||
// (S=standstill, s=stall, d=encoder deviation, o/O=overtemp, L/R=endstops,
|
||||
// e=eeprom). Returns nullopt
|
||||
// (S=standstill, s=stall, d=encoder deviation, o/O=overtemp, L/R=endstops).
|
||||
// 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.
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@ struct TestProgress {
|
|||
};
|
||||
|
||||
// Runs the selected hardware self-tests on a worker thread, off the control
|
||||
// loop. Mirrors CalibrationRoutine's lifecycle: start()/cancel()/running(), live
|
||||
// loop. Mirrors PitchSetupRoutine's lifecycle: start()/cancel()/running(), live
|
||||
// progress(), and takeReport() once finished. The motor/imu/Logger interfaces are
|
||||
// thread-safe; the produced TestReport is handed back for the control thread to
|
||||
// compare against the baseline, write to disk, and surface in the UI.
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ public:
|
|||
d << "DUMP " << L << " state=" << st
|
||||
<< " hsub=0 enabled=" << (homed_ ? 1 : 0)
|
||||
<< " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
|
||||
<< " speed=200000 eeprom_restored=1 soft_margin=5000"
|
||||
<< " speed=200000 center=500000 soft_margin=5000"
|
||||
<< " hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1\n";
|
||||
d << "DUMP " << L << " TMC GCONF=0x00000004 GSTAT=0x00000000"
|
||||
<< " IOIN=0x30000008 TSTEP=0x000fffff RAMPMODE=0x00000000"
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ struct AxisView {
|
|||
long xenc = 0;
|
||||
long enc_err = 0; // live tracking error = xactual - xenc (counts)
|
||||
double target_deg = 0.0; // current scheduler target
|
||||
double counts_per_deg = 0.0; // live scale, for step-size readouts
|
||||
int sg = 0; // SG_RESULT (live, from ST line)
|
||||
int cs = 0; // CS_ACTUAL
|
||||
int pwm = 0; // PWM_SCALE_SUM
|
||||
|
|
@ -140,6 +141,9 @@ struct CameraDeviceView {
|
|||
struct CaptureView {
|
||||
bool present = false;
|
||||
bool active = false;
|
||||
// No scan running, so hand aiming (arrows) and `snapshot` are accepted.
|
||||
bool manual = true;
|
||||
bool snapshot_armed = false; // a snapshot is waiting to fire
|
||||
double image_rate = 0.0; // img/s
|
||||
int camera_count = 0;
|
||||
std::vector<std::string> labels;
|
||||
|
|
@ -281,7 +285,7 @@ struct ActivityView {
|
|||
struct CalibAxisView {
|
||||
bool ok = false;
|
||||
double counts_per_deg = 0;
|
||||
long zero_count = 0;
|
||||
double deg_at_center = 0;
|
||||
double r2 = 0;
|
||||
int n = 0;
|
||||
};
|
||||
|
|
|
|||
4
main.cpp
4
main.cpp
|
|
@ -16,7 +16,7 @@ int main(int argc, char* argv[]) {
|
|||
desc.add_options()
|
||||
("help,h", "show this help")
|
||||
("config,c", po::value<std::string>(), "path to config.ini")
|
||||
("init,i", po::bool_switch(), "run the endstop-finding init sequence")
|
||||
("init,i", po::bool_switch(), "home the gimbal on startup (required: nothing is homed across a power cycle)")
|
||||
("start,s", po::bool_switch(), "start capture automatically")
|
||||
("demo,d", po::bool_switch(), "demo mode (copy placeholder image instead of encoding)")
|
||||
("no-mqtt", po::bool_switch(), "disable MQTT (use a null control channel)")
|
||||
|
|
@ -93,7 +93,7 @@ int main(int argc, char* argv[]) {
|
|||
if (vm["no-tui"].as<bool>()) opts.use_tui = false; // --no-tui wins
|
||||
if (vm.count("log-level")) opts.log_level = vm["log-level"].as<std::string>();
|
||||
if (vm.count("trace")) opts.trace_categories = vm["trace"].as<std::string>();
|
||||
opts.config_path = *cfg_path; // so `gimbal calib` can offer to save back to it
|
||||
opts.config_path = *cfg_path; // so the setup commands can save back to it
|
||||
|
||||
Application app(std::move(cfg), std::move(opts));
|
||||
try {
|
||||
|
|
|
|||
|
|
@ -15,7 +15,10 @@ Wants=network-online.target
|
|||
Type=simple
|
||||
User=ggs
|
||||
WorkingDirectory=/opt/fire_gimbal_control
|
||||
ExecStart=/opt/fire_gimbal_control/fire_gimbal_control --start
|
||||
# --init is REQUIRED, not a convenience: the firmware keeps no state across a
|
||||
# power cycle, so both axes boot into RESET and refuse to move until homed. Without
|
||||
# it the service comes up and sits there, capturing nothing, with no obvious cause.
|
||||
ExecStart=/opt/fire_gimbal_control/fire_gimbal_control --init --start
|
||||
Environment=FGC_CONFIG=/opt/fire_gimbal_control/config.ini
|
||||
# EnvironmentFile=/etc/fire_gimbal_control/credentials.env # FGC_MQTT_USER / FGC_MQTT_PW
|
||||
Restart=on-failure
|
||||
|
|
|
|||
|
|
@ -2,8 +2,11 @@
|
|||
# Launch fire_gimbal_control, resolving the binary relative to this script so it
|
||||
# works regardless of where the repo is checked out. Extra args are forwarded.
|
||||
#
|
||||
# scripts/run.sh --start
|
||||
# scripts/run.sh --init --start # homes first
|
||||
# scripts/run.sh --mock-serial --mock-camera --no-mqtt --start # dev, no hardware
|
||||
#
|
||||
# --init homes the gimbal. The firmware keeps nothing across a power cycle, so
|
||||
# without it the axes stay in RESET and refuse every move.
|
||||
set -euo pipefail
|
||||
|
||||
here="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
|
|
|
|||
|
|
@ -28,7 +28,9 @@ bool rotateCode(int deg, int& code) {
|
|||
|
||||
ImagePipeline::ImagePipeline(IControlChannel& channel, std::function<Orientation()> orientation,
|
||||
Params params)
|
||||
: channel_(channel), orientation_(std::move(orientation)), params_(std::move(params)) {}
|
||||
: channel_(channel), orientation_(std::move(orientation)), params_(std::move(params)) {
|
||||
display_.store(params_.display);
|
||||
}
|
||||
|
||||
ImagePipeline::~ImagePipeline() { stop(); }
|
||||
|
||||
|
|
@ -96,11 +98,27 @@ void ImagePipeline::process(const QueuedFrame& queued) {
|
|||
// src only wraps frame.data, so an unrotated frame costs no copy at all.
|
||||
const cv::Mat& img = rotate ? rotated : src;
|
||||
|
||||
if (params_.display) {
|
||||
cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
|
||||
cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
|
||||
cv::imshow("Display Image", img);
|
||||
cv::waitKey(10);
|
||||
if (display_.load()) {
|
||||
// This runs on the worker thread, so anything escaping here terminates the
|
||||
// whole program - and OpenCV's GUI calls throw whenever there is no usable
|
||||
// display: no X/Wayland, DISPLAY unset or pointing at a dead server (an SSH
|
||||
// session without X11 forwarding), or a highgui built with no GUI backend.
|
||||
// A preview window is never worth taking a tower down for, so failure
|
||||
// disables it and capture carries on untouched.
|
||||
try {
|
||||
cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
|
||||
cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
|
||||
cv::imshow("Display Image", img);
|
||||
cv::waitKey(10);
|
||||
} catch (const std::exception& e) {
|
||||
display_.store(false);
|
||||
LOG_WARN << "local display unavailable; preview disabled (capture is unaffected): "
|
||||
<< e.what();
|
||||
} catch (...) {
|
||||
display_.store(false);
|
||||
LOG_WARN << "local display failed for an unknown reason; preview disabled "
|
||||
"(capture is unaffected)";
|
||||
}
|
||||
}
|
||||
|
||||
const std::string label = out.label.empty() ? ("cam" + std::to_string(frame.cam_id))
|
||||
|
|
|
|||
|
|
@ -193,7 +193,15 @@ struct VimbaCameraSource::Impl {
|
|||
// Begin/end a free-run acquisition session (fresh observers each time). Split out so
|
||||
// trigger() can restart the stream when it stalls. Stats persist across restarts.
|
||||
void startAcquisition() {
|
||||
observers.clear();
|
||||
// Stop first, unconditionally. Clearing `observers` drops the last
|
||||
// references to the FrameObserver objects, and until
|
||||
// StopContinuousImageAcquisition() has returned the SDK's acquisition
|
||||
// thread may still be calling FrameReceived on them - destroying them
|
||||
// under it is a use-after-free, not a leak.
|
||||
stopAcquisition();
|
||||
// A fresh session has delivered nothing, so a watermark from the previous
|
||||
// one would make every genuinely-new frame look stale to trigger().
|
||||
last_captured_ts = 0;
|
||||
int cam_id = 0;
|
||||
for (auto& cam : cameras) {
|
||||
IFrameObserverPtr observer(new FrameObserver(cam, cam_id, stats[cam_id].get()));
|
||||
|
|
@ -256,6 +264,15 @@ void VimbaCameraSource::open() {
|
|||
LOG_INFO << "Opened camera '" << spec.name << "' " << id << " ("
|
||||
<< impl_->identity.back().model << ')';
|
||||
}
|
||||
// Configure each camera ONCE, here. Settings are GenICam features that live on
|
||||
// the device and survive a stop/start of acquisition, so re-applying them
|
||||
// whenever the stream starts is both wasted work (thousands of features) and
|
||||
// wrong in principle: it lets a request to *stream* alter the *configuration*.
|
||||
// A scan frame and a manual snapshot must come off an identically configured
|
||||
// camera, and the only way to guarantee that is to never touch the
|
||||
// configuration in between.
|
||||
for (size_t i = 0; i < impl_->cameras.size() && i < impl_->profiles.size(); ++i)
|
||||
applyProfile(impl_->cameras[i], impl_->profiles[i]);
|
||||
}
|
||||
|
||||
void VimbaCameraSource::close() {
|
||||
|
|
@ -264,9 +281,13 @@ void VimbaCameraSource::close() {
|
|||
impl_->sys.Shutdown();
|
||||
}
|
||||
|
||||
// Start/stop the free-running stream. Configuration is NOT touched here - that
|
||||
// happened at open(), and changes only through setProfile().
|
||||
void VimbaCameraSource::start() {
|
||||
for (size_t i = 0; i < impl_->cameras.size() && i < impl_->profiles.size(); ++i)
|
||||
applyProfile(impl_->cameras[i], impl_->profiles[i]);
|
||||
// Idempotent. Several callers reach here without knowing whether another
|
||||
// already has - `start` (scan), `snapshot`, and --start on the command line -
|
||||
// and re-entering would restart acquisition under the SDK's feet.
|
||||
if (impl_->started) return;
|
||||
impl_->startAcquisition();
|
||||
impl_->started = true;
|
||||
}
|
||||
|
|
@ -279,7 +300,11 @@ bool VimbaCameraSource::setProfile(int camera_index, const CameraProfile& profil
|
|||
}
|
||||
|
||||
impl_->profiles[idx] = profile;
|
||||
if (!impl_->started) return true; // start() will apply it
|
||||
if (!impl_->started) {
|
||||
// Idle: every feature is writable, so apply directly. start() no longer
|
||||
// applies profiles, so skipping this would leave the camera on the old one.
|
||||
return applyProfile(impl_->cameras[idx], profile);
|
||||
}
|
||||
|
||||
// Binning, ROI and pixel format cannot be written while the stream runs, and a
|
||||
// profile may change any of them, so always cycle the stream. Profile switches
|
||||
|
|
@ -299,17 +324,36 @@ bool VimbaCameraSource::setProfile(int camera_index, const CameraProfile& profil
|
|||
}
|
||||
|
||||
void VimbaCameraSource::stop() {
|
||||
if (!impl_->started) return;
|
||||
impl_->stopAcquisition();
|
||||
impl_->started = false;
|
||||
}
|
||||
|
||||
bool VimbaCameraSource::trigger() {
|
||||
// Deliver the freshest complete frame per waypoint. This host's USB3
|
||||
// stream reliably delivers the first frames of a session but stalls after a burst; if
|
||||
// no frame newer than the last capture is available, the stream has stalled - restart
|
||||
// acquisition to unstick it and wait briefly for a fresh frame. Without this, keep-latest
|
||||
// would hand the same stale frame to every remaining waypoint (all overwriting one file).
|
||||
if (impl_->newestTs() <= impl_->last_captured_ts) {
|
||||
// Deliver the freshest complete frame per waypoint. This host's USB3 stream
|
||||
// reliably delivers the first frames of a session but stalls after a burst; if
|
||||
// no frame newer than the last capture is available, the stream has stalled -
|
||||
// restart acquisition to unstick it and wait briefly for a fresh frame. Without
|
||||
// this, keep-latest would hand the same stale frame to every remaining waypoint
|
||||
// (all overwriting one file).
|
||||
const long long newest = impl_->newestTs();
|
||||
|
||||
// A session that has delivered NOTHING is warming up, not stalled. These are
|
||||
// different states that both fail the "no newer frame" test, and conflating
|
||||
// them is expensive: restarting throws away the warm-up and blocks the control
|
||||
// thread for 3 s, so a cold stream would be repeatedly reset just as it was
|
||||
// about to produce its first frame. Return empty-handed instead; the caller
|
||||
// retries every tick.
|
||||
if (newest == 0) {
|
||||
long long now = nowMs();
|
||||
if (now - impl_->last_no_frame_warn_ms > 5000) {
|
||||
LOG_WARN << "camera: no frame yet (stream warming up)";
|
||||
impl_->last_no_frame_warn_ms = now;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
if (newest <= impl_->last_captured_ts) {
|
||||
LOG_TRACE_CAT(LogCat::Camera) << "stream stale; restarting acquisition";
|
||||
for (auto& s : impl_->stats) s->restarts.fetch_add(1);
|
||||
try {
|
||||
|
|
@ -334,11 +378,12 @@ bool VimbaCameraSource::trigger() {
|
|||
}
|
||||
}
|
||||
if (!any) {
|
||||
// The scheduler retries trigger() every tick until a frame is ready, so a brief
|
||||
// warm-up (or a fully stalled stream) would otherwise flood the log. Rate-limit.
|
||||
// Frames HAVE arrived this session (the newest == 0 case returned above), so
|
||||
// this is the restart-did-not-help path. The scheduler retries every tick,
|
||||
// so rate-limit.
|
||||
long long now = nowMs();
|
||||
if (now - impl_->last_no_frame_warn_ms > 5000) {
|
||||
LOG_WARN << "camera: no frame available to capture yet (stream warming up or stalled)";
|
||||
LOG_WARN << "camera: stream stalled and did not recover after a restart";
|
||||
impl_->last_no_frame_warn_ms = now;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#include "fgc/Application.h"
|
||||
|
||||
#include "fgc/CalibrationRoutine.h"
|
||||
#include "fgc/PitchSetup.h"
|
||||
#include "fgc/HeadingFit.h"
|
||||
#include "fgc/CaptureScheduler.h"
|
||||
#include "fgc/CommandParser.h"
|
||||
#include "fgc/DiagParser.h"
|
||||
|
|
@ -62,6 +63,11 @@ namespace fgc {
|
|||
|
||||
namespace {
|
||||
|
||||
// One encoder count in microsteps, rounded up. ENC_CONST is set so X_ENC advances
|
||||
// in 12.8-microstep quanta, which is the finest position the gimbal can actually
|
||||
// reproduce - the tolerance for any "is the count where we asked for it" check.
|
||||
constexpr long kEncoderQuantumCounts = 13;
|
||||
|
||||
// Set by SIGINT/SIGTERM so the control loop exits cleanly and tears the camera down
|
||||
// (StopContinuousImageAcquisition + Close). Without this, a Ctrl-C / kill / systemd
|
||||
// stop skips teardown and leaves the camera acquiring - which wedges it so the NEXT
|
||||
|
|
@ -193,7 +199,7 @@ struct Application::Impl {
|
|||
std::unique_ptr<ImagePipeline> pipeline;
|
||||
std::unique_ptr<CaptureScheduler> scheduler;
|
||||
std::unique_ptr<IUserInterface> ui;
|
||||
std::unique_ptr<CalibrationRoutine> calib;
|
||||
std::unique_ptr<PitchSetupRoutine> calib;
|
||||
std::unique_ptr<TestRunner> tester;
|
||||
unsigned last_diag_seq = 0;
|
||||
ScanGrid grid; // outlives scheduler (holds a reference to it)
|
||||
|
|
@ -234,6 +240,8 @@ struct Application::Impl {
|
|||
// costs one line, not one per frame. Written from the camera delivery
|
||||
// thread(s) via the orientation supplier, hence atomic.
|
||||
std::atomic<bool> imu_pitch_warned_{false};
|
||||
// The TUI heading wizard is aiming by hand; blocks a scan from starting.
|
||||
bool heading_setup_active_ = false;
|
||||
|
||||
std::atomic<bool> running{true};
|
||||
std::mutex cmd_mutex;
|
||||
|
|
@ -443,9 +451,9 @@ struct Application::Impl {
|
|||
return out;
|
||||
}
|
||||
|
||||
// `imu profile [<type|label>]`. The MTi's XKF profile decides whether heading
|
||||
// uses the magnetometer, so switching it is how you move between a compass-
|
||||
// referenced heading and the no-mag profile `gimbal calib` leaves behind.
|
||||
// `imu profile [<type|label>]`. The MTi's XKF profile decides whether its
|
||||
// heading uses the magnetometer. Note this does not affect the gimbal's own
|
||||
// heading, which comes from the encoder via `gimbal heading setup`.
|
||||
void imuProfile(const std::string& arg) {
|
||||
if (arg.empty()) {
|
||||
LOG_INFO << "XKF profiles (* = active): " << imuProfileList();
|
||||
|
|
@ -532,8 +540,8 @@ struct Application::Impl {
|
|||
}
|
||||
|
||||
// `imu` command family, mirroring handleGimbal: the operator-facing surface of
|
||||
// IImuSource. `imu norotation` + `imu zero` are the quick heading calibration
|
||||
// that gimbal calib runs internally, without the multi-minute sweep.
|
||||
// IImuSource. `imu norotation` + `imu zero` correct and re-zero the MTi's own
|
||||
// heading; the gimbal's heading is independent of both.
|
||||
void handleImu(const std::string& line) {
|
||||
std::istringstream iss(line);
|
||||
std::vector<std::string> tok;
|
||||
|
|
@ -608,15 +616,19 @@ struct Application::Impl {
|
|||
MotorTelemetry t = motor->telemetry();
|
||||
s.gimbal.present = motor->connected();
|
||||
s.gimbal.pitch_present = t.pitch_present;
|
||||
// Yaw degrees are true compass bearings in [0,360); pitch is a signed
|
||||
// elevation about level, so it must not wrap.
|
||||
auto fillAxis = [](AxisView& v, const char* label, const AxisTelemetry& a,
|
||||
const AxisMap& map, long target_counts) {
|
||||
const AxisMap& map, long target_counts, bool wrap) {
|
||||
auto deg = [&](long counts) { return wrap ? map.headingDeg(counts) : map.toDeg(counts); };
|
||||
v.label = label;
|
||||
v.state = a.state;
|
||||
v.deg = map.toDeg(a.xenc);
|
||||
v.deg = deg(a.xenc);
|
||||
v.xactual = a.xactual;
|
||||
v.xenc = a.xenc;
|
||||
v.enc_err = a.xactual - a.xenc; // live tracking error (counts)
|
||||
v.target_deg = map.toDeg(target_counts);
|
||||
v.target_deg = deg(target_counts);
|
||||
v.counts_per_deg = map.counts_per_deg;
|
||||
v.sg = a.sg;
|
||||
v.cs = a.cs;
|
||||
v.pwm = a.pwm;
|
||||
|
|
@ -631,27 +643,27 @@ struct Application::Impl {
|
|||
};
|
||||
long yaw_tgt = scheduler ? scheduler->yawTargetCounts() : 0;
|
||||
long pitch_tgt = scheduler ? scheduler->pitchTargetCounts() : 0;
|
||||
fillAxis(s.gimbal.yaw, "YAW", t.yaw, cfg.geometry.yaw, yaw_tgt);
|
||||
fillAxis(s.gimbal.pitch, "PITCH", t.pitch, cfg.geometry.pitch, pitch_tgt);
|
||||
fillAxis(s.gimbal.yaw, "YAW", t.yaw, cfg.geometry.yaw, yaw_tgt, true);
|
||||
fillAxis(s.gimbal.pitch, "PITCH", t.pitch, cfg.geometry.pitch, pitch_tgt, false);
|
||||
|
||||
// Homing travel limits come from the last firmware dump (lim_neg/lim_pos);
|
||||
// convert the endstop counts to degrees via the per-axis geometry.
|
||||
DumpData dd = parseDump(motor->lastDump());
|
||||
if (dd.valid) {
|
||||
auto fillLimits = [](AxisView& v, const DumpAxis& a, const AxisMap& map) {
|
||||
auto fillLimits = [](AxisView& v, const DumpAxis& a, const AxisMap& map, bool wrap) {
|
||||
v.has_limits = true;
|
||||
v.lim_neg = a.lim_neg;
|
||||
v.lim_pos = a.lim_pos;
|
||||
v.lim_neg_deg = map.toDeg(a.lim_neg);
|
||||
v.lim_pos_deg = map.toDeg(a.lim_pos);
|
||||
v.lim_neg_deg = wrap ? map.headingDeg(a.lim_neg) : map.toDeg(a.lim_neg);
|
||||
v.lim_pos_deg = wrap ? map.headingDeg(a.lim_pos) : map.toDeg(a.lim_pos);
|
||||
v.has_track = true;
|
||||
v.hold_corrections = a.hold_corrections;
|
||||
v.hold_peak_dev = a.hold_peak_dev;
|
||||
v.hold_cumulative_dev = a.hold_cumulative_dev;
|
||||
};
|
||||
for (const auto& a : dd.axes) {
|
||||
if (a.axis == 'Y') fillLimits(s.gimbal.yaw, a, cfg.geometry.yaw);
|
||||
else if (a.axis == 'P') fillLimits(s.gimbal.pitch, a, cfg.geometry.pitch);
|
||||
if (a.axis == 'Y') fillLimits(s.gimbal.yaw, a, cfg.geometry.yaw, true);
|
||||
else if (a.axis == 'P') fillLimits(s.gimbal.pitch, a, cfg.geometry.pitch, false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -753,6 +765,10 @@ struct Application::Impl {
|
|||
// --- Camera / capture ---
|
||||
s.capture.present = true;
|
||||
s.capture.active = scheduler && scheduler->captureActive();
|
||||
// Manual = no scan running. Drives what the key bar advertises; the
|
||||
// enforcement is in requireManual(), not in the UI.
|
||||
s.capture.manual = manualMode();
|
||||
s.capture.snapshot_armed = scheduler && scheduler->snapshotPending();
|
||||
s.capture.image_rate = scheduler ? scheduler->imageRate() : 0.0;
|
||||
s.capture.camera_count = camera ? camera->cameraCount() : 0;
|
||||
s.capture.labels.clear();
|
||||
|
|
@ -926,7 +942,7 @@ struct Application::Impl {
|
|||
|
||||
// Pending yes/no: persist the just-applied calibration as the new default?
|
||||
if (calib_save_pending_)
|
||||
a.prompt = "Save this calibration to config as the new default? (y / n)";
|
||||
a.prompt = "Save the new [Motor] calibration to config as the default? (y / n)";
|
||||
}
|
||||
|
||||
void publishSnapshot() {
|
||||
|
|
@ -971,15 +987,21 @@ struct Application::Impl {
|
|||
|
||||
motor->sendCommand("ENABLE Y");
|
||||
motor->sendCommand("ENABLE P");
|
||||
// Tell the firmware where to anchor the homed midpoint and how much
|
||||
// clearance to keep from each endstop, BEFORE homing: finishHoming() uses
|
||||
// the centre count to re-origin, so a late send would apply to the next
|
||||
// home, not this one.
|
||||
sendAnchorConfig();
|
||||
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.
|
||||
// An axis re-homed from READY is still READY for a moment, 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 having waited for nothing.
|
||||
const auto t0 = std::chrono::steady_clock::now();
|
||||
while (running && !cancel_procedure_ && std::chrono::steady_clock::now() < t0 + 3s) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
|
|
@ -1013,77 +1035,136 @@ struct Application::Impl {
|
|||
return;
|
||||
}
|
||||
// Re-anchor the yaw zero to an endstop now that the homed limits are known.
|
||||
if (homed) applyYawHomeZero();
|
||||
if (homed) applyHomedTravel();
|
||||
// Production move speeds for subsequent MOVE commands.
|
||||
motor->sendCommand("SPEED Y 50000");
|
||||
motor->sendCommand("SPEED P 150000");
|
||||
LOG_INFO << "Gimbal init finished";
|
||||
}
|
||||
|
||||
// With [Motor] yaw_home_zero = low|high, redefine the yaw degree origin from
|
||||
// the freshly homed endstops: the chosen endstop becomes 0 deg and degrees
|
||||
// rise toward the other limit (forced positive, so it lands near +360). The
|
||||
// counts_per_deg magnitude is kept from config/calibration; only the zero and
|
||||
// sign change, so the yaw zero is fixed by homing and never needs a separate
|
||||
// calibration. Pitch is left untouched.
|
||||
void applyYawHomeZero() {
|
||||
// Tell the firmware the anchor point and endstop clearance for each axis.
|
||||
// The centre count is what finishHoming() re-origins onto, so all counts stay
|
||||
// positive and reproducible across power cycles; the margin is what
|
||||
// clampTarget() keeps clear of each hard limit.
|
||||
void sendAnchorConfig() {
|
||||
motor->sendCommand("CENTER Y " + std::to_string(cfg.geometry.yaw.center_count));
|
||||
motor->sendCommand("CENTER P " + std::to_string(cfg.geometry.pitch.center_count));
|
||||
motor->sendCommand("MARGIN Y " + std::to_string(cfg.yaw_soft_margin_counts));
|
||||
motor->sendCommand("MARGIN P " + std::to_string(cfg.pitch_soft_margin_counts));
|
||||
}
|
||||
|
||||
// Derive each axis's reachable degree window from the freshly homed soft
|
||||
// limits. These are deliberately NOT configurable: the firmware's limits are
|
||||
// the only place they can be correct, and a stale configured range would
|
||||
// either advertise travel that does not exist or hide travel that does.
|
||||
//
|
||||
// Also verifies that the firmware actually adopted the anchor and margin we
|
||||
// sent. With no EEPROM the limits can only come from a home in this power
|
||||
// cycle, so a midpoint that is not center_count means the CENTER command did
|
||||
// not take effect - firmware predating it answers "ERR unknown command" and
|
||||
// homes to its own compiled default, which would leave every published
|
||||
// bearing silently wrong.
|
||||
void applyHomedTravel() {
|
||||
using namespace std::chrono_literals;
|
||||
if (cfg.yaw_home_zero == YawHomeZero::Off) return;
|
||||
|
||||
// The homed limits live in the firmware DUMP; pull a fresh one and wait
|
||||
// briefly for it to arrive with yaw endstops populated.
|
||||
// briefly for it to arrive.
|
||||
motor->sendCommand("DUMP");
|
||||
long lo = 0, hi = 0, margin = 0;
|
||||
bool got = false;
|
||||
DumpData dd;
|
||||
const auto deadline = std::chrono::steady_clock::now() + 3s;
|
||||
while (running && std::chrono::steady_clock::now() < deadline) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
publishSnapshot();
|
||||
DumpData dd = parseDump(motor->lastDump());
|
||||
if (dd.valid)
|
||||
for (const auto& a : dd.axes)
|
||||
if (a.axis == 'Y' && a.lim_pos != a.lim_neg) {
|
||||
lo = a.lim_neg; hi = a.lim_pos; margin = a.soft_margin; got = true;
|
||||
}
|
||||
if (got) break;
|
||||
dd = parseDump(motor->lastDump());
|
||||
if (dd.valid) break;
|
||||
}
|
||||
if (!got) {
|
||||
LOG_WARN << "yaw home-zero: no homed yaw limits in dump; keeping configured zero";
|
||||
if (!dd.valid) {
|
||||
LOG_WARN << "homed travel: no firmware dump; keeping the previous degree limits";
|
||||
return;
|
||||
}
|
||||
if (margin <= 0)
|
||||
LOG_WARN << "yaw home-zero: firmware reported no soft_margin (update firmware); "
|
||||
"anchoring at the hard homing limit instead of the soft limit";
|
||||
|
||||
const double mag = std::fabs(cfg.geometry.yaw.counts_per_deg);
|
||||
if (mag < 1e-9) {
|
||||
LOG_WARN << "yaw home-zero: yaw_counts_per_deg ~0; calibrate first; keeping zero";
|
||||
return;
|
||||
bool any = false;
|
||||
for (const auto& a : dd.axes) {
|
||||
if (a.lim_pos == a.lim_neg) continue; // that axis has not homed
|
||||
const bool is_yaw = (a.axis == 'Y');
|
||||
AxisMap& map = is_yaw ? cfg.geometry.yaw : cfg.geometry.pitch;
|
||||
const char* name = is_yaw ? "yaw" : "pitch";
|
||||
const long want_margin =
|
||||
is_yaw ? cfg.yaw_soft_margin_counts : cfg.pitch_soft_margin_counts;
|
||||
|
||||
// One encoder quantum of slack: ENC_CONST makes X_ENC advance in 12.8
|
||||
// microstep steps, so the midpoint can land a count or so off.
|
||||
const long mid = a.lim_neg + (a.lim_pos - a.lim_neg) / 2;
|
||||
if (std::labs(mid - map.center_count) > kEncoderQuantumCounts) {
|
||||
LOG_ERROR << "homed " << name << " midpoint is " << mid << " but ["
|
||||
<< "Motor] " << name << "_center_count is " << map.center_count
|
||||
<< " -- the firmware did not adopt CENTER (is it older than this "
|
||||
"host?). Refusing to derive travel limits; headings would be "
|
||||
"wrong.";
|
||||
continue;
|
||||
}
|
||||
|
||||
long margin = a.soft_margin;
|
||||
if (margin <= 0) {
|
||||
LOG_WARN << "homed " << name
|
||||
<< ": firmware reported no soft_margin (update firmware); "
|
||||
"anchoring at the hard homing limit instead";
|
||||
margin = 0;
|
||||
} else if (margin != want_margin) {
|
||||
// Trust the firmware: it is what is actually clamping the axis, so
|
||||
// deriving from our value would advertise travel it will not allow.
|
||||
LOG_WARN << "homed " << name << ": firmware soft_margin " << margin
|
||||
<< " != configured " << want_margin
|
||||
<< "; using the firmware's value";
|
||||
}
|
||||
|
||||
const long lo_soft = a.lim_neg + margin;
|
||||
const long hi_soft = a.lim_pos - margin;
|
||||
const double d_lo = map.toDeg(lo_soft);
|
||||
const double d_hi = map.toDeg(hi_soft);
|
||||
map.min_deg = std::min(d_lo, d_hi);
|
||||
map.max_deg = std::max(d_lo, d_hi);
|
||||
any = true;
|
||||
|
||||
if (is_yaw)
|
||||
LOG_INFO << "homed yaw travel: " << map.min_deg << ".." << map.max_deg
|
||||
<< " deg (headings " << wrap360(map.min_deg) << ".."
|
||||
<< wrap360(map.max_deg) << " true), centre " << map.center_count
|
||||
<< " = " << map.deg_at_center << " deg";
|
||||
else
|
||||
LOG_INFO << "homed pitch travel: " << map.min_deg << ".." << map.max_deg
|
||||
<< " deg, centre " << map.center_count << " = " << map.deg_at_center
|
||||
<< " deg";
|
||||
}
|
||||
// Anchor at the soft limits (the actual reachable travel), not the hard
|
||||
// homing endstops, so the rightmost *reachable* position reads exactly 0.
|
||||
const long lo_soft = lo + margin;
|
||||
const long hi_soft = hi - margin;
|
||||
const bool zero_high = (cfg.yaw_home_zero == YawHomeZero::High);
|
||||
const long zero_cnt = zero_high ? hi_soft : lo_soft;
|
||||
const long other_cnt = zero_high ? lo_soft : hi_soft;
|
||||
// Sign so the other endstop sits at positive degrees (rises toward ~+360).
|
||||
const double cpd = (other_cnt > zero_cnt) ? mag : -mag;
|
||||
const double range_deg = std::fabs(static_cast<double>(other_cnt - zero_cnt)) / mag;
|
||||
|
||||
AxisMap& y = cfg.geometry.yaw;
|
||||
y.zero_count = zero_cnt;
|
||||
y.counts_per_deg = cpd;
|
||||
y.min_deg = 0.0;
|
||||
y.max_deg = range_deg;
|
||||
if (!any) return;
|
||||
if (scheduler) scheduler->setGeometry(cfg.geometry); // re-target the scan with it
|
||||
auditScanReachability();
|
||||
}
|
||||
|
||||
LOG_INFO << "yaw zero anchored at " << (zero_high ? "high" : "low")
|
||||
<< "-count endstop: 0.0.." << range_deg << " deg (zero_count=" << zero_cnt
|
||||
<< ", counts_per_deg=" << cpd << ")";
|
||||
// Report scan waypoints that fall in the yaw arc the gimbal cannot reach, once
|
||||
// per homing rather than once per pass. The scheduler skips them at run time;
|
||||
// without this the only symptom would be a grid quietly shorter than the CSV.
|
||||
void auditScanReachability() {
|
||||
if (grid.empty()) return;
|
||||
std::size_t bad = 0;
|
||||
for (const auto& p : grid.points())
|
||||
if (!cfg.geometry.yaw.headingToCounts(p.yaw_deg)) ++bad;
|
||||
if (bad == 0) return;
|
||||
LOG_WARN << bad << " of " << grid.size()
|
||||
<< " scan waypoints lie outside the reachable yaw arc ("
|
||||
<< wrap360(cfg.geometry.yaw.min_deg) << ".."
|
||||
<< wrap360(cfg.geometry.yaw.max_deg) << " true) and will be skipped";
|
||||
}
|
||||
|
||||
void startCapture() {
|
||||
// Heading setup owns the gimbal by hand. Starting a scan under it would
|
||||
// move the axis between a sighting and the bearing being typed for it,
|
||||
// silently poisoning the fit with a position nobody looked through.
|
||||
if (heading_setup_active_) {
|
||||
LOG_WARN << "Capture not started: heading setup in progress "
|
||||
"(finish it, or 'gimbal heading cancel')";
|
||||
return;
|
||||
}
|
||||
// The scheduler arms regardless, but a scan can't progress until the axes
|
||||
// are homed: before READY the firmware leaves coils disabled and clamps
|
||||
// MOVE targets to a coarse bench guard, so no waypoint is ever reached and
|
||||
|
|
@ -1104,6 +1185,123 @@ struct Application::Impl {
|
|||
LOG_INFO << "Capture stopped";
|
||||
}
|
||||
|
||||
// `gimbal heading fit <counts>:<deg> ...` / `gimbal heading offset <deg>`.
|
||||
//
|
||||
// `fit` is what the TUI wizard emits after the operator has sighted landmarks,
|
||||
// but it is a first-class command in its own right: on a headless tower an
|
||||
// operator who already knows two (encoder position, true bearing) pairs can
|
||||
// type it directly.
|
||||
void handleHeading(const std::vector<std::string>& tok) {
|
||||
const std::string what = tok.size() > 2 ? lowerStr(tok[2]) : "";
|
||||
if (what == "setup") {
|
||||
// The TUI drives the interaction (arrow keys and typed bearings exist
|
||||
// only there); this just claims the gimbal so a scan cannot start
|
||||
// underneath it and move the axis between a sighting and its bearing.
|
||||
if (procedureBusy()) {
|
||||
LOG_WARN << "gimbal heading setup: " << procedureName() << " in progress";
|
||||
return;
|
||||
}
|
||||
if (!requireManual("gimbal heading setup")) return;
|
||||
heading_setup_active_ = true;
|
||||
LOG_INFO << "heading setup: aim at a landmark whose true bearing you know, "
|
||||
"Space to record an image, Enter to enter its bearing. Two "
|
||||
"landmarks minimum, further apart is better.";
|
||||
return;
|
||||
}
|
||||
if (what == "cancel") {
|
||||
if (heading_setup_active_) LOG_INFO << "heading setup cancelled";
|
||||
heading_setup_active_ = false;
|
||||
return;
|
||||
}
|
||||
if (what == "offset") {
|
||||
double off = 0;
|
||||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal heading offset <deg>"; return; }
|
||||
try { off = std::stod(tok[3]); } catch (...) {
|
||||
LOG_WARN << "gimbal heading offset: bad number '" << tok[3] << "'";
|
||||
return;
|
||||
}
|
||||
AxisMap& y = cfg.geometry.yaw;
|
||||
const double old = y.deg_at_center;
|
||||
y.deg_at_center = wrap360(old + off);
|
||||
applyYawMapChange();
|
||||
LOG_INFO << "yaw heading at centre " << old << " -> " << y.deg_at_center
|
||||
<< " deg true (offset " << off << ")";
|
||||
return;
|
||||
}
|
||||
if (what != "fit") {
|
||||
LOG_WARN << "usage: gimbal heading <setup|fit <counts>:<deg> ...|offset <deg>>";
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<HeadingSighting> sightings;
|
||||
std::string err;
|
||||
if (!parseHeadingSightings({tok.begin() + 3, tok.end()}, sightings, err)) {
|
||||
LOG_WARN << "gimbal heading fit: " << err;
|
||||
return;
|
||||
}
|
||||
const HeadingFitResult r = fitHeading(sightings, cfg.geometry.yaw.center_count);
|
||||
if (!r.ok) {
|
||||
LOG_WARN << "gimbal heading fit: " << r.error;
|
||||
return;
|
||||
}
|
||||
|
||||
AxisMap& y = cfg.geometry.yaw;
|
||||
LOG_INFO << "yaw heading fit over " << r.n << " sightings spanning " << r.span_deg
|
||||
<< " deg: counts_per_deg " << y.counts_per_deg << " -> " << r.counts_per_deg
|
||||
<< ", heading at centre " << y.deg_at_center << " -> " << r.deg_at_center
|
||||
<< " deg true (R^2=" << r.r2 << ")";
|
||||
y.counts_per_deg = r.counts_per_deg;
|
||||
y.deg_at_center = r.deg_at_center;
|
||||
applyYawMapChange();
|
||||
|
||||
heading_setup_active_ = false;
|
||||
// Offer to persist, through the same activity-strip prompt the pitch setup
|
||||
// uses - the operator answers y/n without leaving the dashboard.
|
||||
if (!opts.config_path.empty()) calib_save_pending_ = true;
|
||||
}
|
||||
|
||||
// Re-derive everything that depends on the yaw map after it changes: the
|
||||
// reachable degree window (which is in degrees, so it moves with the map) and
|
||||
// the scheduler's copy used to target scan waypoints.
|
||||
void applyYawMapChange() {
|
||||
applyHomedTravel();
|
||||
if (scheduler) scheduler->setGeometry(cfg.geometry);
|
||||
}
|
||||
|
||||
// Capture one image now, outside any scan: the operator's reference shot while
|
||||
// aiming by hand. It goes through the scheduler rather than poking the camera
|
||||
// directly so it produces exactly what a scan waypoint produces - same file,
|
||||
// same CamEvent on MQTT.
|
||||
void takeSnapshot() {
|
||||
if (!requireManual("snapshot")) return;
|
||||
if (!camera || !scheduler) { LOG_WARN << "snapshot: no camera configured"; return; }
|
||||
MotorTelemetry t = motor->telemetry();
|
||||
if (!t.yaw.ready() || (t.pitch_present && !t.pitch.ready())) {
|
||||
LOG_WARN << "snapshot: gimbal not homed (press h / run 'gimbal home' first)";
|
||||
return;
|
||||
}
|
||||
// The camera is stopped outside a scan; leave it running afterwards so a
|
||||
// burst of snapshots does not pay the start-up cost each time. stopCapture()
|
||||
// and shutdown stop it as before.
|
||||
camera->start();
|
||||
scheduler->requestSnapshot();
|
||||
LOG_INFO << "snapshot requested";
|
||||
}
|
||||
|
||||
// Manual mode is simply "no scan running" - derived, never a separate flag an
|
||||
// operator could strand. Hand aiming and a scan are mutually exclusive: both
|
||||
// drive the same axes, and a nudge mid-sweep would silently corrupt the
|
||||
// waypoint the next frame is published under.
|
||||
bool manualMode() const { return !scheduler || !scheduler->captureActive(); }
|
||||
|
||||
// Guard for the hand-aiming commands. Returns false (having said why) when a
|
||||
// scan owns the gimbal.
|
||||
bool requireManual(const char* what) {
|
||||
if (manualMode()) return true;
|
||||
LOG_WARN << "ignored '" << what << "': a scan is running (press x / type 'stop' first)";
|
||||
return false;
|
||||
}
|
||||
|
||||
// `trace off|none`, `trace all`, or `trace <cat> [on|off]` — toggle the
|
||||
// verbatim wire-trace categories (serial/mqtt/camera/control).
|
||||
void handleTrace(const Command& c) {
|
||||
|
|
@ -1160,13 +1358,19 @@ struct Application::Impl {
|
|||
std::string t;
|
||||
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
|
||||
if (tok.size() < 2) {
|
||||
LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
|
||||
"speed|setpos|status|dump|calib|raw> (self-test: 'test')";
|
||||
LOG_WARN << "usage: gimbal <move|steps|nudge|jog|heading|pitch|home|stop|reset|"
|
||||
"enable|disable|speed|setpos|status|dump|raw> (self-test: 'test')";
|
||||
return;
|
||||
}
|
||||
std::string sub = tok[1];
|
||||
for (char& ch : sub) ch = static_cast<char>(std::tolower((unsigned char)ch));
|
||||
|
||||
// Hand aiming is manual-mode only: these fight a running scan for the
|
||||
// same axes, and a move mid-sweep mislabels the next published frame.
|
||||
if (sub == "move" || sub == "steps" || sub == "nudge" || sub == "jog") {
|
||||
if (!requireManual(("gimbal " + sub).c_str())) return;
|
||||
}
|
||||
|
||||
if (sub == "move") {
|
||||
double yaw_deg = 0, pitch_deg = 0;
|
||||
if (!parsePair(tok, 2, yaw_deg, pitch_deg)) {
|
||||
|
|
@ -1205,8 +1409,25 @@ struct Application::Impl {
|
|||
long target = cur + static_cast<long>(pct / 100.0 * range);
|
||||
LOG_INFO << "gimbal nudge " << axis << " " << pct << "% -> MOVE " << axis << " " << target;
|
||||
motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||||
} else if (sub == "calib") {
|
||||
startCalibration();
|
||||
} else if (sub == "jog") {
|
||||
// Relative move in raw counts. `nudge` is a percent of travel and
|
||||
// `steps` is absolute; neither can express the sub-degree steps the
|
||||
// heading wizard needs, and counts are what the encoder resolves.
|
||||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal jog <yaw|pitch> <+/-counts>"; return; }
|
||||
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
||||
long delta = 0;
|
||||
try { delta = std::stol(tok[3]); } catch (...) { LOG_WARN << "gimbal jog: bad count"; return; }
|
||||
MotorTelemetry tel = motor->telemetry();
|
||||
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
||||
long target = cur + delta;
|
||||
LOG_INFO << "gimbal jog " << axis << " " << delta << " counts -> MOVE " << axis
|
||||
<< " " << target;
|
||||
motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||||
} else if (sub == "heading") {
|
||||
handleHeading(tok);
|
||||
} else if (sub == "pitch") {
|
||||
if (tok.size() > 2 && lowerStr(tok[2]) == "setup") startPitchSetup();
|
||||
else LOG_WARN << "usage: gimbal pitch setup";
|
||||
} else if (sub == "dump") {
|
||||
LOG_INFO << "requesting firmware dump...";
|
||||
motor->sendCommand("DUMP");
|
||||
|
|
@ -1246,11 +1467,25 @@ struct Application::Impl {
|
|||
}
|
||||
}
|
||||
|
||||
void startCalibration() {
|
||||
if (!imu) { LOG_WARN << "gimbal calib requires the IMU (set [Features] enable_imu)"; return; }
|
||||
if (calib && calib->running()) { LOG_WARN << "calibration already running (gimbal stop to cancel)"; return; }
|
||||
void startPitchSetup() {
|
||||
// Pitch is the one axis that genuinely needs the IMU: level is defined by
|
||||
// gravity, and nothing else on the tower can measure it. Yaw does not -
|
||||
// `gimbal heading setup` uses landmarks instead.
|
||||
if (!imu) {
|
||||
LOG_WARN << "gimbal pitch setup requires the IMU (set [Features] enable_imu); "
|
||||
"yaw does not - use 'gimbal heading setup'";
|
||||
return;
|
||||
}
|
||||
if (calib && calib->running()) {
|
||||
LOG_WARN << "pitch setup already running (press Esc, or type 'stop', to cancel)";
|
||||
return;
|
||||
}
|
||||
if (heading_setup_active_) {
|
||||
LOG_WARN << "pitch setup: heading setup in progress ('gimbal heading cancel' first)";
|
||||
return;
|
||||
}
|
||||
stopCapture(); // free the serial link + don't fight the routine's moves
|
||||
calib = std::make_unique<CalibrationRoutine>(*motor, *imu, cfg.geometry);
|
||||
calib = std::make_unique<PitchSetupRoutine>(*motor, *imu, cfg.geometry);
|
||||
calib->start();
|
||||
}
|
||||
|
||||
|
|
@ -1258,25 +1493,26 @@ struct Application::Impl {
|
|||
void saveCalibration() {
|
||||
if (!calib_save_pending_) return;
|
||||
calib_save_pending_ = false;
|
||||
if (saveMotorCalibration(opts.config_path, cfg.geometry))
|
||||
LOG_INFO << "calibration saved as the new default in " << opts.config_path;
|
||||
if (saveMotorGeometry(opts.config_path, cfg.geometry, cfg.yaw_soft_margin_counts,
|
||||
cfg.pitch_soft_margin_counts))
|
||||
LOG_INFO << "[Motor] calibration saved as the new default in " << opts.config_path;
|
||||
else
|
||||
LOG_WARN << "could not write calibration to " << opts.config_path;
|
||||
LOG_WARN << "could not write [Motor] calibration to " << opts.config_path;
|
||||
}
|
||||
|
||||
void cancelCalibration() {
|
||||
void cancelPitchSetup() {
|
||||
if (calib && calib->running()) {
|
||||
LOG_INFO << "cancelling calibration";
|
||||
LOG_INFO << "cancelling pitch setup";
|
||||
calib->cancel();
|
||||
motor->sendCommand("STOP ALL");
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Procedure lock-out (test / calibration / homing are mutually exclusive) ----
|
||||
enum class Procedure { None, Homing, Calibration, Test };
|
||||
// ---- Procedure lock-out (test / pitch setup / homing are mutually exclusive) ----
|
||||
enum class Procedure { None, Homing, PitchSetup, Test };
|
||||
Procedure activeProcedure() const {
|
||||
if (tester && tester->running()) return Procedure::Test;
|
||||
if (calib && calib->running()) return Procedure::Calibration;
|
||||
if (calib && calib->running()) return Procedure::PitchSetup;
|
||||
if (homing_active_.load()) return Procedure::Homing;
|
||||
return Procedure::None;
|
||||
}
|
||||
|
|
@ -1284,7 +1520,7 @@ struct Application::Impl {
|
|||
const char* procedureName() const {
|
||||
switch (activeProcedure()) {
|
||||
case Procedure::Test: return "test";
|
||||
case Procedure::Calibration: return "calibration";
|
||||
case Procedure::PitchSetup: return "pitch setup";
|
||||
case Procedure::Homing: return "homing";
|
||||
default: return "none";
|
||||
}
|
||||
|
|
@ -1297,8 +1533,8 @@ struct Application::Impl {
|
|||
tester->cancel();
|
||||
motor->sendCommand("STOP ALL");
|
||||
break;
|
||||
case Procedure::Calibration:
|
||||
cancelCalibration();
|
||||
case Procedure::PitchSetup:
|
||||
cancelPitchSetup();
|
||||
break;
|
||||
case Procedure::Homing:
|
||||
LOG_INFO << "cancelling homing";
|
||||
|
|
@ -1475,13 +1711,13 @@ struct Application::Impl {
|
|||
if (ax.axis == 'P') v.pitch_present = true;
|
||||
dst->ok = ax.ok;
|
||||
dst->counts_per_deg = ax.counts_per_deg;
|
||||
dst->zero_count = ax.zero_count;
|
||||
dst->deg_at_center = ax.deg_at_center;
|
||||
dst->r2 = ax.r2;
|
||||
dst->n = ax.n;
|
||||
char b[96];
|
||||
std::snprintf(b, sizeof(b), "%s cpd=%.3f zero=%ld R2=%.4f (n=%d)%s",
|
||||
std::snprintf(b, sizeof(b), "%s cpd=%.3f deg@ctr=%.3f R2=%.4f (n=%d)%s",
|
||||
ax.axis == 'P' ? "PITCH" : "YAW", ax.counts_per_deg,
|
||||
ax.zero_count, ax.r2, ax.n, ax.ok ? "" : " FIT FAILED");
|
||||
ax.deg_at_center, ax.r2, ax.n, ax.ok ? "" : " FIT FAILED");
|
||||
last_calib_summary.emplace_back(b);
|
||||
}
|
||||
last_calib_view = v;
|
||||
|
|
@ -1576,8 +1812,8 @@ struct Application::Impl {
|
|||
// Commands that would disturb a running procedure (motion, capture, a second
|
||||
// long op, or a DUMP over the same serial link). Ignored while busy.
|
||||
static bool interferingVerb(const std::string& verb) {
|
||||
return verb == "gimbal" || verb == "start" || verb == "calib" ||
|
||||
verb == "test" || verb == "refresh";
|
||||
return verb == "gimbal" || verb == "start" || verb == "test" ||
|
||||
verb == "refresh" || verb == "snapshot";
|
||||
}
|
||||
|
||||
void handleCommand(const std::string& line) {
|
||||
|
|
@ -1605,14 +1841,18 @@ struct Application::Impl {
|
|||
} else if (c.verb == "imu") {
|
||||
handleImu(line);
|
||||
} else if (c.verb == "calib") {
|
||||
// Answer to the "save calibration?" prompt (activity-strip y/n keys).
|
||||
// Answer to the "save to config?" prompt (activity-strip y/n keys).
|
||||
// Shared by `gimbal pitch setup` and `gimbal heading fit`: both change
|
||||
// the [Motor] map and both offer the same choice.
|
||||
if (c.device == "save") saveCalibration();
|
||||
else if (c.device == "discard") {
|
||||
calib_save_pending_ = false;
|
||||
LOG_INFO << "calibration kept for this session only (not saved to config)";
|
||||
LOG_INFO << "kept for this session only (not saved to config)";
|
||||
}
|
||||
} else if (c.verb == "exit") {
|
||||
running = false;
|
||||
} else if (c.verb == "snapshot") {
|
||||
takeSnapshot();
|
||||
} else if (c.verb == "start") {
|
||||
startCapture();
|
||||
} else if (c.verb == "stop") {
|
||||
|
|
@ -1655,7 +1895,16 @@ struct Application::Impl {
|
|||
}
|
||||
else if (c.option == "jxlq" && c.has_value) pipeline->setDistance(c.value);
|
||||
else if (c.option == "jxle" && c.has_value) pipeline->setEffort(static_cast<int>(c.value));
|
||||
else if (c.option == "display") pipeline->setDisplay(c.value != 0.0);
|
||||
else if (c.option == "display") {
|
||||
const bool on = c.value != 0.0;
|
||||
pipeline->setDisplay(on);
|
||||
// Confirm it: the preview only repaints when a frame goes
|
||||
// through the pipeline, so with no capture running there is
|
||||
// nothing on screen and silence is indistinguishable from the
|
||||
// command having been rejected.
|
||||
LOG_INFO << "local display " << (on ? "on" : "off")
|
||||
<< " (updates on the next captured frame)";
|
||||
}
|
||||
else LOG_WARN << "unknown 'set camera' option: " << c.option;
|
||||
} else if (c.device == "fps" && c.has_value) {
|
||||
scheduler->setImageRate(c.value);
|
||||
|
|
@ -1742,9 +1991,10 @@ struct Application::Impl {
|
|||
// Orientation supplier, called by ImagePipeline::submit() as each frame
|
||||
// arrives. The two axes come from different sources on purpose:
|
||||
//
|
||||
// yaw - the encoder. IMU yaw drifts and depends on the heading reset
|
||||
// and the XKF profile, whereas encoder yaw is calibrated against
|
||||
// the IMU by `gimbal calib` and then stays put.
|
||||
// yaw - the encoder, as a true compass bearing in [0,360). IMU yaw
|
||||
// drifts and depends on the heading reset and the XKF profile,
|
||||
// whereas encoder yaw is anchored to the homed centre and tied to
|
||||
// north by `gimbal heading setup`, and then stays put.
|
||||
// pitch - the IMU. The encoder only measures the axis relative to the
|
||||
// gimbal's own base, so a leaning mast is invisible to it; the
|
||||
// IMU measures against gravity, which is the true elevation the
|
||||
|
|
@ -1753,7 +2003,7 @@ struct Application::Impl {
|
|||
*channel,
|
||||
[this] {
|
||||
MotorTelemetry t = motor->telemetry();
|
||||
Orientation o{static_cast<float>(cfg.geometry.yaw.toDeg(t.yaw.xenc)),
|
||||
Orientation o{static_cast<float>(cfg.geometry.yaw.headingDeg(t.yaw.xenc)),
|
||||
static_cast<float>(cfg.geometry.pitch.toDeg(t.pitch.xenc))};
|
||||
if (!imu) return o; // IMU disabled in config: encoder pitch, as before
|
||||
if (auto s = imu->sample()) {
|
||||
|
|
@ -1813,13 +2063,14 @@ struct Application::Impl {
|
|||
});
|
||||
|
||||
if (opts.init) {
|
||||
runInitSequence(); // homes, then re-anchors the yaw zero
|
||||
} else if (cfg.yaw_home_zero != YawHomeZero::Off) {
|
||||
// No homing this launch: the firmware restores its homed limits from
|
||||
// EEPROM to READY on boot, so re-derive the same yaw zero from them.
|
||||
// This makes the homing-anchored zero persist across software restarts
|
||||
// without re-homing. Self-guards (no-op + note) if not yet homed.
|
||||
applyYawHomeZero();
|
||||
runInitSequence(); // homes, then derives the travel window
|
||||
} else {
|
||||
// No homing this launch. The firmware keeps nothing across a power
|
||||
// cycle, so this only finds limits when the gimbal was already homed
|
||||
// earlier in this session (e.g. the host restarted, the board did not).
|
||||
// Self-guards to a warning when there is no dump to read.
|
||||
sendAnchorConfig();
|
||||
applyHomedTravel();
|
||||
}
|
||||
if (opts.start) startCapture();
|
||||
|
||||
|
|
|
|||
|
|
@ -38,6 +38,10 @@ void CaptureScheduler::setCaptureActive(bool active) {
|
|||
capture_active_ = active;
|
||||
moving_ = false; // restart the move/settle cycle cleanly
|
||||
settled_at_ms_ = 0;
|
||||
// Drop any armed snapshot: a scan starting means the operator moved on, and
|
||||
// keeping it would fire an image at some unrelated later moment when the scan
|
||||
// stops - attributed to whatever the gimbal happened to be pointing at then.
|
||||
snapshot_pending_ = false;
|
||||
resetTimer();
|
||||
}
|
||||
void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }
|
||||
|
|
@ -69,6 +73,45 @@ bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
|
|||
std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
|
||||
}
|
||||
|
||||
void CaptureScheduler::requestSnapshot() {
|
||||
snapshot_pending_ = true;
|
||||
snapshot_settled_ms_ = 0;
|
||||
}
|
||||
|
||||
// One frame, on request, through exactly the path a scan waypoint takes: the
|
||||
// same standstill + settle wait, the same camera trigger, and therefore the same
|
||||
// .jxl on disk and the same CamEvent on MQTT. Reusing the path rather than
|
||||
// triggering the camera directly is the point - a snapshot that differed from a
|
||||
// scan frame would be useless as a reference for aiming.
|
||||
void CaptureScheduler::serviceSnapshot(const MotorTelemetry& t) {
|
||||
const bool standstill = t.yaw.standstill && (!pitch_present_ || t.pitch.standstill);
|
||||
if (!standstill) {
|
||||
snapshot_settled_ms_ = 0; // still slewing (e.g. a jog is in flight)
|
||||
return;
|
||||
}
|
||||
if (snapshot_settled_ms_ == 0) {
|
||||
snapshot_settled_ms_ = now_ms_();
|
||||
return;
|
||||
}
|
||||
const long long since_settled = now_ms_() - snapshot_settled_ms_;
|
||||
if (since_settled < settle_wait_ms_) return;
|
||||
|
||||
if (camera_.trigger()) {
|
||||
LOG_INFO << "snapshot captured";
|
||||
snapshot_pending_ = false;
|
||||
return;
|
||||
}
|
||||
// No fresh frame yet. Give up on the same budget a waypoint gets - measured
|
||||
// from settling, as the message says, not from the keypress: the operator may
|
||||
// have pressed Space mid-slew, and counting the slew against the camera would
|
||||
// abandon a snapshot the camera never had a chance at.
|
||||
if (since_settled > settle_wait_ms_ + frame_timeout_ms_) {
|
||||
LOG_WARN << "snapshot: no frame within " << frame_timeout_ms_
|
||||
<< " ms of settling; press Space again";
|
||||
snapshot_pending_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
void CaptureScheduler::tick() {
|
||||
// 1. Remote control input.
|
||||
ControlCommand cmd = channel_.poll();
|
||||
|
|
@ -86,9 +129,15 @@ void CaptureScheduler::tick() {
|
|||
MotorTelemetry t = motor_.telemetry();
|
||||
pitch_present_ = t.pitch_present;
|
||||
|
||||
// 3. Capture cycle. Only ControlCode 0 (sweep) and 1 (directed) act.
|
||||
// 3. Manual snapshot. Only outside a scan, so a keypress can never inject a
|
||||
// frame into a running sweep or fight it for the settle window.
|
||||
if (!capture_active_) {
|
||||
if (snapshot_pending_) serviceSnapshot(t);
|
||||
return;
|
||||
}
|
||||
|
||||
// 4. 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;
|
||||
|
||||
|
|
@ -99,18 +148,49 @@ void CaptureScheduler::tick() {
|
|||
if (control_code_ == 0) {
|
||||
if (grid_.empty()) return;
|
||||
const ScanPoint& p = grid_.current();
|
||||
yaw_target_ = geometry_.yaw.toCounts(p.yaw_deg);
|
||||
// Yaw travel is under a full turn, so some bearings are unreachable.
|
||||
// Skip such a waypoint rather than clamping to the nearest limit,
|
||||
// which would image a direction nobody asked for and label it with
|
||||
// the one they did.
|
||||
auto yc = geometry_.yaw.headingToCounts(p.yaw_deg);
|
||||
if (!yc) {
|
||||
if (!unreachable_warned_) {
|
||||
unreachable_warned_ = true;
|
||||
LOG_WARN << "scan waypoint " << (grid_.index() + 1) << "/" << grid_.size()
|
||||
<< " heading " << p.yaw_deg
|
||||
<< " deg is outside the reachable yaw arc; skipping it (further "
|
||||
"unreachable waypoints are skipped silently)";
|
||||
}
|
||||
grid_.advance();
|
||||
resetTimer();
|
||||
return;
|
||||
}
|
||||
yaw_target_ = *yc;
|
||||
pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
|
||||
// One INFO line per move so the operator can follow the scan in the
|
||||
// log without enabling the control wire-trace.
|
||||
LOG_INFO << "scan move " << (grid_.index() + 1) << "/" << grid_.size()
|
||||
<< ": yaw " << p.yaw_deg << " pitch " << p.pitch_deg << " (deg)";
|
||||
} else { // ControlCode 1: directed yaw, pitch held at current position.
|
||||
double heading = 0.0;
|
||||
try {
|
||||
yaw_target_ = geometry_.yaw.toCounts(std::stod(target_heading_));
|
||||
heading = std::stod(target_heading_);
|
||||
} catch (const std::exception&) {
|
||||
return; // bad heading; wait for a valid one
|
||||
}
|
||||
auto yc = geometry_.yaw.headingToCounts(heading);
|
||||
if (!yc) {
|
||||
// Hold position and say so: silently pointing at the nearest limit
|
||||
// would publish CamEvents claiming a bearing we cannot reach.
|
||||
if (!directed_unreachable_warned_) {
|
||||
directed_unreachable_warned_ = true;
|
||||
LOG_WARN << "target heading " << heading
|
||||
<< " deg is outside the reachable yaw arc; holding position";
|
||||
}
|
||||
return;
|
||||
}
|
||||
directed_unreachable_warned_ = false;
|
||||
yaw_target_ = *yc;
|
||||
pitch_target_ = t.pitch.xenc;
|
||||
LOG_INFO << "directed move: heading " << target_heading_ << " (deg)";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
#include "fgc/Config.h"
|
||||
#include "fgc/Logger.h"
|
||||
#include "fgc/Paths.h"
|
||||
|
||||
#include <cmath>
|
||||
|
|
@ -17,6 +18,8 @@ extern "C" {
|
|||
|
||||
namespace fgc {
|
||||
|
||||
namespace fs = std::filesystem;
|
||||
|
||||
namespace {
|
||||
|
||||
std::string get(const std::map<std::string, std::string>& kv, const std::string& key,
|
||||
|
|
@ -362,22 +365,17 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
|
|||
|
||||
cfg.ui.enable_tui = getBool(kv, "UI.enable_tui", cfg.ui.enable_tui);
|
||||
|
||||
// [Motor]: operator-calibrated degrees<->counts maps (see Geometry).
|
||||
// [Motor]: the degrees<->counts maps, anchored on the homed mechanical centre
|
||||
// (see Geometry). min_deg/max_deg are NOT configured - they are derived from
|
||||
// the firmware's homed soft limits, the only place they can be correct.
|
||||
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.yaw.center_count = getLong(kv, "Motor.yaw_center_count", cfg.geometry.yaw.center_count);
|
||||
cfg.geometry.yaw.deg_at_center = getDouble(kv, "Motor.yaw_heading_at_center", cfg.geometry.yaw.deg_at_center);
|
||||
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);
|
||||
{
|
||||
std::string hz = get(kv, "Motor.yaw_home_zero", "off");
|
||||
for (auto& ch : hz) ch = static_cast<char>(std::tolower((unsigned char)ch));
|
||||
cfg.yaw_home_zero = hz == "high" ? YawHomeZero::High
|
||||
: hz == "low" ? YawHomeZero::Low
|
||||
: YawHomeZero::Off;
|
||||
}
|
||||
cfg.geometry.pitch.center_count = getLong(kv, "Motor.pitch_center_count", cfg.geometry.pitch.center_count);
|
||||
cfg.geometry.pitch.deg_at_center = getDouble(kv, "Motor.pitch_angle_at_center", cfg.geometry.pitch.deg_at_center);
|
||||
cfg.yaw_soft_margin_counts = getLong(kv, "Motor.yaw_soft_margin_counts", cfg.yaw_soft_margin_counts);
|
||||
cfg.pitch_soft_margin_counts = getLong(kv, "Motor.pitch_soft_margin_counts", cfg.pitch_soft_margin_counts);
|
||||
cfg.enc_error_warn_counts = getLong(kv, "Motor.enc_error_warn_counts",
|
||||
cfg.enc_error_warn_counts);
|
||||
|
||||
|
|
@ -395,6 +393,40 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
|
|||
if (cfg.serial.baud == 0)
|
||||
throw std::runtime_error("Serial.baud must be > 0");
|
||||
|
||||
// A zero scale makes every degree map to the centre and every count read back
|
||||
// as the centre - the axis would look calibrated and point nowhere.
|
||||
if (std::abs(cfg.geometry.yaw.counts_per_deg) < 1e-9)
|
||||
throw std::runtime_error("Motor.yaw_counts_per_deg must be non-zero");
|
||||
if (std::abs(cfg.geometry.pitch.counts_per_deg) < 1e-9)
|
||||
throw std::runtime_error("Motor.pitch_counts_per_deg must be non-zero");
|
||||
// The centre count exists so the whole working range stays positive; a
|
||||
// non-positive one defeats the point and can drive the firmware negative.
|
||||
if (cfg.geometry.yaw.center_count <= 0)
|
||||
throw std::runtime_error("Motor.yaw_center_count must be > 0");
|
||||
if (cfg.geometry.pitch.center_count <= 0)
|
||||
throw std::runtime_error("Motor.pitch_center_count must be > 0");
|
||||
if (cfg.yaw_soft_margin_counts < 0)
|
||||
throw std::runtime_error("Motor.yaw_soft_margin_counts must be >= 0");
|
||||
if (cfg.pitch_soft_margin_counts < 0)
|
||||
throw std::runtime_error("Motor.pitch_soft_margin_counts must be >= 0");
|
||||
|
||||
// Keys from the pre-centre-anchoring layout. Silently ignoring them would
|
||||
// leave an operator convinced they had configured a heading that is in fact
|
||||
// coming from somewhere else entirely - the exact failure this layout removes.
|
||||
static const char* kRemovedMotorKeys[] = {
|
||||
"Motor.yaw_zero_count", "Motor.pitch_zero_count", "Motor.yaw_min_deg",
|
||||
"Motor.yaw_max_deg", "Motor.pitch_min_deg", "Motor.pitch_max_deg",
|
||||
"Motor.yaw_home_zero",
|
||||
};
|
||||
std::string stale;
|
||||
for (const char* k : kRemovedMotorKeys)
|
||||
if (kv.count(k)) stale += (stale.empty() ? "" : ", ") + std::string(k + 6);
|
||||
if (!stale.empty())
|
||||
LOG_WARN << "[Motor] ignoring keys removed in the centre-anchored layout: " << stale
|
||||
<< " -- travel limits are now derived from homing, and the heading "
|
||||
"origin comes from yaw_heading_at_center (set it with "
|
||||
"'gimbal heading setup')";
|
||||
|
||||
return cfg;
|
||||
}
|
||||
|
||||
|
|
@ -470,7 +502,8 @@ std::string updateIniSectionKeys(const std::string& contents, const std::string&
|
|||
return out.str();
|
||||
}
|
||||
|
||||
bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
|
||||
bool saveMotorGeometry(const std::string& path, const Geometry& geo, long yaw_margin,
|
||||
long pitch_margin) {
|
||||
if (path.empty()) return false;
|
||||
std::ifstream f(path);
|
||||
if (!f) return false;
|
||||
|
|
@ -483,26 +516,43 @@ bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
|
|||
std::snprintf(b, sizeof(b), "%.10g", v);
|
||||
return std::string(b);
|
||||
};
|
||||
// Persist the full per-axis map, not just the fitted counts_per_deg/zero_count:
|
||||
// min_deg/max_deg are part of the geometry and consumers (e.g. travel-range
|
||||
// estimates) rely on them, so writing them keeps the [Motor] block complete and
|
||||
// self-consistent rather than depending on pre-existing lines.
|
||||
// The whole [Motor] block, so it stays complete and self-consistent rather
|
||||
// than depending on which lines happened to be there already. min_deg/max_deg
|
||||
// are deliberately absent: they are derived from homing, not configured.
|
||||
const std::vector<std::pair<std::string, std::string>> kv = {
|
||||
{"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
|
||||
{"yaw_zero_count", std::to_string(geo.yaw.zero_count)},
|
||||
{"yaw_min_deg", num(geo.yaw.min_deg)},
|
||||
{"yaw_max_deg", num(geo.yaw.max_deg)},
|
||||
{"yaw_center_count", std::to_string(geo.yaw.center_count)},
|
||||
{"yaw_heading_at_center", num(geo.yaw.deg_at_center)},
|
||||
{"yaw_soft_margin_counts", std::to_string(yaw_margin)},
|
||||
{"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
|
||||
{"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
|
||||
{"pitch_min_deg", num(geo.pitch.min_deg)},
|
||||
{"pitch_max_deg", num(geo.pitch.max_deg)},
|
||||
{"pitch_center_count", std::to_string(geo.pitch.center_count)},
|
||||
{"pitch_angle_at_center", num(geo.pitch.deg_at_center)},
|
||||
{"pitch_soft_margin_counts", std::to_string(pitch_margin)},
|
||||
};
|
||||
const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
|
||||
|
||||
std::ofstream out(path, std::ios::trunc);
|
||||
if (!out) return false;
|
||||
out << updated;
|
||||
return static_cast<bool>(out);
|
||||
// Write a sibling temp file and rename over the original: a truncating write
|
||||
// that dies partway leaves config.ini destroyed, and three commands rewrite
|
||||
// this file while the program is running.
|
||||
std::error_code ec;
|
||||
const fs::path target(path);
|
||||
const fs::path tmp = target.parent_path() / (target.filename().string() + ".tmp");
|
||||
{
|
||||
std::ofstream out(tmp, std::ios::trunc | std::ios::binary);
|
||||
if (!out) return false;
|
||||
out << updated;
|
||||
out.flush();
|
||||
if (!out) {
|
||||
fs::remove(tmp, ec);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
fs::rename(tmp, target, ec);
|
||||
if (ec) {
|
||||
fs::remove(tmp, ec);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -155,11 +155,11 @@ DumpData parseDump(const std::string& block) {
|
|||
ax.state_name = stateName(static_cast<int>(asInt(get(m, "state", "-1"))));
|
||||
ax.hsub = static_cast<int>(asInt(get(m, "hsub", "0")));
|
||||
ax.enabled = asInt(get(m, "enabled", "0")) != 0;
|
||||
ax.eeprom_restored = asInt(get(m, "eeprom_restored", "0")) != 0;
|
||||
ax.has_encoder = asInt(get(m, "has_encoder", "0")) != 0;
|
||||
ax.lim_neg = asInt(get(m, "lim_neg", "0"));
|
||||
ax.lim_pos = asInt(get(m, "lim_pos", "0"));
|
||||
ax.soft_margin = asInt(get(m, "soft_margin", "0"));
|
||||
ax.center_count = asInt(get(m, "center", "0"));
|
||||
ax.hold_target = asInt(get(m, "hold_target", "0"));
|
||||
ax.speed = asInt(get(m, "speed", "0"));
|
||||
ax.hold_corrections = asInt(get(m, "hold_corr", "0"));
|
||||
|
|
@ -198,7 +198,6 @@ std::vector<std::string> formatDump(const DumpData& d) {
|
|||
<< " enabled=" << (ax.enabled ? 1 : 0)
|
||||
<< " limits=[" << ax.lim_neg << ".." << ax.lim_pos << "]"
|
||||
<< " hold_target=" << ax.hold_target
|
||||
<< " eeprom_restored=" << (ax.eeprom_restored ? 1 : 0)
|
||||
<< " has_encoder=" << (ax.has_encoder ? 1 : 0);
|
||||
out.push_back(os.str());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,14 +5,45 @@
|
|||
|
||||
namespace fgc {
|
||||
|
||||
double wrap360(double deg) {
|
||||
double w = std::fmod(deg, 360.0);
|
||||
if (w < 0.0) w += 360.0;
|
||||
// fmod of a tiny negative can round to exactly 360.0 after the += above.
|
||||
if (w >= 360.0) w = 0.0;
|
||||
return w;
|
||||
}
|
||||
|
||||
double wrap180(double deg) {
|
||||
double w = wrap360(deg);
|
||||
return w >= 180.0 ? w - 360.0 : w;
|
||||
}
|
||||
|
||||
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);
|
||||
const double lo = std::min(min_deg, max_deg);
|
||||
const double hi = std::max(min_deg, max_deg);
|
||||
const double clamped = std::clamp(deg, lo, hi);
|
||||
return std::lround(static_cast<double>(center_count) +
|
||||
(clamped - deg_at_center) * 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;
|
||||
if (std::abs(counts_per_deg) < 1e-9) return deg_at_center;
|
||||
return deg_at_center +
|
||||
(static_cast<double>(counts) - static_cast<double>(center_count)) / counts_per_deg;
|
||||
}
|
||||
|
||||
double AxisMap::headingDeg(long counts) const { return wrap360(toDeg(counts)); }
|
||||
|
||||
std::optional<long> AxisMap::headingToCounts(double heading_deg) const {
|
||||
if (std::abs(counts_per_deg) < 1e-9) return std::nullopt;
|
||||
// The travel is under a full turn, so wrapping the offset into [-180, 180)
|
||||
// yields the one candidate that could be reachable - there is never a second
|
||||
// solution to choose between.
|
||||
const double deg = deg_at_center + wrap180(heading_deg - deg_at_center);
|
||||
const double lo = std::min(min_deg, max_deg);
|
||||
const double hi = std::max(min_deg, max_deg);
|
||||
if (deg < lo || deg > hi) return std::nullopt;
|
||||
return toCounts(deg);
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -0,0 +1,99 @@
|
|||
#include "fgc/HeadingFit.h"
|
||||
|
||||
#include "fgc/Calibration.h"
|
||||
#include "fgc/Geometry.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count) {
|
||||
HeadingFitResult r;
|
||||
r.n = static_cast<int>(sightings.size());
|
||||
|
||||
if (sightings.size() < 2) {
|
||||
r.error = "need at least two sightings (aim at a second landmark)";
|
||||
return r;
|
||||
}
|
||||
|
||||
// Sort by encoder position so the bearings can be unwrapped along the axis.
|
||||
// Sighting order is whatever the operator found convenient; the axis is the
|
||||
// only thing that varies monotonically.
|
||||
std::vector<HeadingSighting> pts = sightings;
|
||||
std::sort(pts.begin(), pts.end(),
|
||||
[](const HeadingSighting& a, const HeadingSighting& b) { return a.counts < b.counts; });
|
||||
|
||||
// Unwrap so a set spanning north fits as a straight line instead of folding
|
||||
// at 360 (e.g. 350 and 10 become 350 and 370, not 350 and 10).
|
||||
std::vector<std::pair<double, double>> xy; // x = bearing, y = counts
|
||||
xy.reserve(pts.size());
|
||||
double prev = wrap180(pts.front().heading_deg);
|
||||
for (std::size_t i = 0; i < pts.size(); ++i) {
|
||||
const double deg = (i == 0) ? prev : unwrapNear(prev, pts[i].heading_deg);
|
||||
prev = deg;
|
||||
xy.emplace_back(deg, static_cast<double>(pts[i].counts));
|
||||
}
|
||||
|
||||
auto [lo, hi] = std::minmax_element(
|
||||
xy.begin(), xy.end(), [](const auto& a, const auto& b) { return a.first < b.first; });
|
||||
r.span_deg = hi->first - lo->first;
|
||||
if (r.span_deg < kMinHeadingBaselineDeg) {
|
||||
r.error = "sightings are only " + std::to_string(static_cast<int>(r.span_deg)) +
|
||||
" deg apart; use landmarks at least " +
|
||||
std::to_string(static_cast<int>(kMinHeadingBaselineDeg)) +
|
||||
" deg apart (further is better)";
|
||||
return r;
|
||||
}
|
||||
|
||||
const LinearFit fit = linearFit(xy);
|
||||
if (!fit.ok) {
|
||||
r.error = "degenerate sightings (all at the same bearing)";
|
||||
return r;
|
||||
}
|
||||
// Distinct bearings at the same encoder position: the axis did not move
|
||||
// between sightings, so one of the bearings is wrong.
|
||||
if (std::abs(fit.slope) < 1e-6) {
|
||||
r.error = "sightings give no encoder travel between bearings; did the gimbal move?";
|
||||
return r;
|
||||
}
|
||||
|
||||
r.counts_per_deg = fit.slope;
|
||||
// The map is anchored on the homed centre, so what gets stored is the bearing
|
||||
// that centre points at - solve the fitted line for it.
|
||||
r.deg_at_center = wrap360((static_cast<double>(center_count) - fit.intercept) / fit.slope);
|
||||
r.r2 = fit.r2;
|
||||
r.ok = true;
|
||||
return r;
|
||||
}
|
||||
|
||||
bool parseHeadingSightings(const std::vector<std::string>& tokens,
|
||||
std::vector<HeadingSighting>& out, std::string& error) {
|
||||
out.clear();
|
||||
for (const std::string& t : tokens) {
|
||||
const auto colon = t.find(':');
|
||||
if (colon == std::string::npos) {
|
||||
error = "expected <counts>:<deg>, got '" + t + "'";
|
||||
return false;
|
||||
}
|
||||
HeadingSighting s;
|
||||
try {
|
||||
std::size_t used = 0;
|
||||
s.counts = std::stol(t.substr(0, colon), &used);
|
||||
if (used != colon) throw std::invalid_argument("trailing");
|
||||
const std::string deg = t.substr(colon + 1);
|
||||
s.heading_deg = std::stod(deg, &used);
|
||||
if (used != deg.size()) throw std::invalid_argument("trailing");
|
||||
} catch (const std::exception&) {
|
||||
error = "bad sighting '" + t + "' (expected <counts>:<deg>)";
|
||||
return false;
|
||||
}
|
||||
out.push_back(s);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -24,25 +24,54 @@ std::string verbOf(const std::string& syntax) {
|
|||
const std::vector<HelpSection>& helpCatalog() {
|
||||
// clang-format off
|
||||
static const std::vector<HelpSection> catalog = {
|
||||
{"Positioning", "Aim the gimbal. 'move' is degrees; 'steps' is raw encoder counts.", {
|
||||
{"Positioning",
|
||||
"Aim the gimbal. Yaw is a TRUE BEARING (0 = north); pitch is elevation (0 = level).", {
|
||||
{"gimbal move <yaw>,<pitch>",
|
||||
"Point the gimbal at an absolute heading/elevation in DEGREES.", {
|
||||
"Converts to encoder counts with the operator calibration and moves both",
|
||||
"axes together, soft-clamped to the travel limits.",
|
||||
"Example: gimbal move 30,-10 (yaw 30 deg, pitch -10 deg)"}},
|
||||
"Point at an absolute bearing/elevation in DEGREES.", {
|
||||
"Converts to encoder counts with the [Motor] calibration and moves both",
|
||||
"axes together. Yaw travel is under a full turn, so bearings in the",
|
||||
"unreachable arc are refused rather than clamped to the nearest limit.",
|
||||
"Manual mode only - refused while a scan is running.",
|
||||
"Example: gimbal move 30,-10 (bearing 30 deg, 10 deg below level)"}},
|
||||
{"gimbal steps <yaw>,<pitch>",
|
||||
"Move both axes to absolute encoder COUNTS (no degree conversion).", {
|
||||
"Example: gimbal steps 100000,250000"}},
|
||||
{"gimbal nudge <yaw|pitch> <+/-pct>",
|
||||
"Relative step move by a percent of the axis travel.", {
|
||||
"Relative move by a percent of the homed axis travel.", {
|
||||
"The arrow keys do this when no overlay is open; inside an overlay they",
|
||||
"scroll instead.",
|
||||
"scroll instead. Manual mode only.",
|
||||
"Example: gimbal nudge yaw -5"}},
|
||||
{"gimbal jog <yaw|pitch> <+/-counts>",
|
||||
"Relative move in raw encoder counts.", {
|
||||
"Finer than 'nudge' (a percent of travel) and relative unlike 'steps'.",
|
||||
"Below ~13 counts the encoder cannot resolve the difference, so a smaller",
|
||||
"step moves the motor to a position it cannot reproduce.",
|
||||
"Example: gimbal jog yaw -25"}},
|
||||
{"gimbal heading setup",
|
||||
"Tie encoder counts to true north by sighting known landmarks (TUI).", {
|
||||
"Opens a guided pane (dashboard key 'H'): aim with the arrow keys, Tab",
|
||||
"cycles coarse/fine/very-fine steps, Space records an image, Enter enters",
|
||||
"that landmark's true bearing. Two landmarks minimum - the further apart",
|
||||
"the better. Needs no IMU, which is the point: IMU yaw drifts and not",
|
||||
"every tower has one."}},
|
||||
{"gimbal heading fit <counts>:<deg> <counts>:<deg> [...]",
|
||||
"Fit the yaw map directly from known (encoder position, bearing) pairs.", {
|
||||
"What the 'H' wizard emits, usable by hand on a headless tower. Two pairs",
|
||||
"fit exactly; more are least-squares fitted and report R^2. Pairs closer",
|
||||
"than 10 deg apart are refused - the fit would look perfect and be wrong.",
|
||||
"Example: gimbal heading fit 480000:45.0 520000:115.0"}},
|
||||
{"gimbal heading offset <deg>",
|
||||
"Shift the whole yaw map by a known amount and save it.", {
|
||||
"For when the tower is found to be off by a fixed bearing and re-sighting",
|
||||
"landmarks is not worth it.",
|
||||
"Example: gimbal heading offset -2.5"}},
|
||||
{"gimbal home [y|p]",
|
||||
"Run the endstop-finding home sequence (both axes, or one).", {
|
||||
"Example: gimbal home / gimbal home y"}},
|
||||
{"gimbal stop [y|p|all]",
|
||||
"Stop motion immediately (also cancels a running calibration).", {}},
|
||||
"Stop motion immediately.", {
|
||||
"To cancel a running procedure use the bare 'stop' (or 'cancel', or Esc):",
|
||||
"the whole 'gimbal' family is ignored while one is in progress."}},
|
||||
{"gimbal speed <y|p> <vel>",
|
||||
"Set the max slew speed (counts/s) for an axis.", {}},
|
||||
{"gimbal reset [y|p] / gimbal enable|disable <y|p> / gimbal setpos <y|p> <v>",
|
||||
|
|
@ -59,9 +88,9 @@ const std::vector<HelpSection>& helpCatalog() {
|
|||
"list. Re-reads first, so it never reports a stale profile."}},
|
||||
{"imu profile [<type|label>]",
|
||||
"Select the XKF filter profile (persists on the device).", {
|
||||
"The profile decides whether heading uses the magnetometer: 'gimbal calib'",
|
||||
"leaves the device on a no-mag one, where yaw is drift-corrected but not",
|
||||
"compass-referenced. Typical MTi profiles are General, High_mag_dep,",
|
||||
"The profile decides whether heading uses the magnetometer. Nothing here",
|
||||
"affects the gimbal's own heading: that comes from the encoder, tied to",
|
||||
"north by 'gimbal heading setup'. Typical MTi profiles are General, High_mag_dep,",
|
||||
"Dynamic and VRU_general; run with no argument for this device's real list.",
|
||||
"Example: imu profile General / imu profile 39"}},
|
||||
{"imu zero",
|
||||
|
|
@ -85,11 +114,13 @@ const std::vector<HelpSection>& helpCatalog() {
|
|||
"token names a config profile. Each run is compared to a pinned baseline",
|
||||
"and the previous run. Esc cancels.",
|
||||
"Example: test gimbal homing / test list / test baseline"}},
|
||||
{"gimbal calib",
|
||||
"Calibrate steps<->degrees using the IMU (auto-homes; ~minutes).", {
|
||||
"Fits pitch then yaw, applies the result to the session and writes",
|
||||
"logs/calib_*.log. Leaves the IMU on a no-mag XKF profile. 'gimbal stop'",
|
||||
"cancels."}},
|
||||
{"gimbal pitch setup",
|
||||
"Calibrate PITCH steps<->degrees against gravity, using the IMU.", {
|
||||
"Auto-homes if needed, sweeps pitch dwelling at each step, fits, then",
|
||||
"parks level. Writes logs/pitch_setup_*.log and offers to save. Takes",
|
||||
"minutes; 'stop' or Esc cancels. Requires the IMU - level is defined by",
|
||||
"gravity and nothing else on the tower can measure it. Yaw is NOT touched:",
|
||||
"use 'gimbal heading setup'."}},
|
||||
{"gimbal status",
|
||||
"Ask the firmware to emit one telemetry (ST) line now.", {}},
|
||||
{"gimbal raw \"<command>\"",
|
||||
|
|
@ -124,6 +155,15 @@ const std::vector<HelpSection>& helpCatalog() {
|
|||
"An overlay takes the full terminal height. Esc closes it; with none open,",
|
||||
"Esc cancels a running procedure. 'm' shows broker state, per-topic traffic",
|
||||
"and the recent MQTT messages, including any dropped while offline."}},
|
||||
{"Space",
|
||||
"Take one image now and publish its CamEvent (manual mode only).", {
|
||||
"Goes through the same settle-and-trigger path a scan waypoint uses, so",
|
||||
"the file and the CamEvent are indistinguishable from a scan frame.",
|
||||
"Refused while a scan is running."}},
|
||||
{"H",
|
||||
"Open the heading-setup wizard (see 'gimbal heading setup').", {
|
||||
"Manual mode only. Blocks a scan from starting until it is finished or",
|
||||
"cancelled with Esc."}},
|
||||
{"Up/Down, j/k",
|
||||
"Scroll the open overlay one line (Help: change section).", {
|
||||
"The scrollbar on the right shows how much is off-screen."}},
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
#include "fgc/CalibrationRoutine.h"
|
||||
#include "fgc/PitchSetup.h"
|
||||
|
||||
#include "fgc/Calibration.h"
|
||||
#include "fgc/DumpParser.h"
|
||||
|
|
@ -23,16 +23,16 @@ using clock = std::chrono::steady_clock;
|
|||
void msleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); }
|
||||
} // namespace
|
||||
|
||||
CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
||||
PitchSetupRoutine::PitchSetupRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
||||
CalibParams params)
|
||||
: motor_(motor), imu_(imu), initial_(std::move(initial)), params_(params) {}
|
||||
|
||||
CalibrationRoutine::~CalibrationRoutine() {
|
||||
PitchSetupRoutine::~PitchSetupRoutine() {
|
||||
cancel();
|
||||
if (thread_.joinable()) thread_.join();
|
||||
}
|
||||
|
||||
bool CalibrationRoutine::start() {
|
||||
bool PitchSetupRoutine::start() {
|
||||
if (running_.exchange(true)) {
|
||||
LOG_WARN << "calibration already running";
|
||||
return false;
|
||||
|
|
@ -43,26 +43,26 @@ bool CalibrationRoutine::start() {
|
|||
return true;
|
||||
}
|
||||
|
||||
void CalibrationRoutine::cancel() { cancel_ = true; }
|
||||
void PitchSetupRoutine::cancel() { cancel_ = true; }
|
||||
|
||||
std::optional<Geometry> CalibrationRoutine::takeResult() {
|
||||
std::optional<Geometry> PitchSetupRoutine::takeResult() {
|
||||
std::lock_guard<std::mutex> lk(result_mutex_);
|
||||
std::optional<Geometry> r;
|
||||
r.swap(result_);
|
||||
return r;
|
||||
}
|
||||
|
||||
CalibProgress CalibrationRoutine::progress() const {
|
||||
CalibProgress PitchSetupRoutine::progress() const {
|
||||
std::lock_guard<std::mutex> lk(progress_mutex_);
|
||||
return progress_;
|
||||
}
|
||||
|
||||
CalibReport CalibrationRoutine::report() const {
|
||||
CalibReport PitchSetupRoutine::report() const {
|
||||
std::lock_guard<std::mutex> lk(result_mutex_);
|
||||
return report_;
|
||||
}
|
||||
|
||||
void CalibrationRoutine::setProgress(char axis, int step, int total, const char* phase) {
|
||||
void PitchSetupRoutine::setProgress(char axis, int step, int total, const char* phase) {
|
||||
std::lock_guard<std::mutex> lk(progress_mutex_);
|
||||
progress_.running = true;
|
||||
progress_.axis = axis;
|
||||
|
|
@ -107,25 +107,19 @@ bool waitHomed(IMotorController& motor, int timeout_ms, const std::atomic<bool>&
|
|||
return false;
|
||||
}
|
||||
|
||||
// Dwell `dwell_ms` sampling the IMU; return the (circular for yaw) mean of the
|
||||
// requested Euler angle in degrees.
|
||||
double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms,
|
||||
// Dwell `dwell_ms` sampling the IMU; return the mean measured pitch in degrees.
|
||||
// A plain arithmetic mean is right here: pitch is a gravity-referenced elevation
|
||||
// over a ~40 deg range, so it cannot wrap the way a heading can.
|
||||
double dwellAndMeasure(IImuSource& imu, int dwell_ms, int sample_ms,
|
||||
const std::atomic<bool>& cancel) {
|
||||
std::vector<double> vals;
|
||||
const auto end = clock::now() + std::chrono::milliseconds(dwell_ms);
|
||||
while (clock::now() < end) {
|
||||
if (cancel) break;
|
||||
if (auto s = imu.sample())
|
||||
vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg);
|
||||
if (auto s = imu.sample()) vals.push_back(s->pitch_deg);
|
||||
msleep(sample_ms);
|
||||
}
|
||||
if (vals.empty()) return 0.0;
|
||||
if (axis == 'Y') {
|
||||
// Robust to a wrap within the dwell, and already in the device's own
|
||||
// -180..180. The sweep's per-sample unwrapNear below is what keeps the
|
||||
// fit continuous, so no range shift is needed - or wanted - here.
|
||||
return circularMeanDeg(vals);
|
||||
}
|
||||
double m = 0;
|
||||
for (double v : vals) m += v;
|
||||
return m / vals.size();
|
||||
|
|
@ -133,9 +127,9 @@ double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms,
|
|||
|
||||
} // namespace
|
||||
|
||||
void CalibrationRoutine::run() {
|
||||
void PitchSetupRoutine::run() {
|
||||
struct Done {
|
||||
CalibrationRoutine* self;
|
||||
PitchSetupRoutine* self;
|
||||
~Done() {
|
||||
self->running_ = false;
|
||||
std::lock_guard<std::mutex> lk(self->progress_mutex_);
|
||||
|
|
@ -144,13 +138,13 @@ void CalibrationRoutine::run() {
|
|||
} done{this};
|
||||
|
||||
const int N = params_.positions;
|
||||
LOG_INFO << "=== gimbal calibration starting ===";
|
||||
LOG_INFO << "=== gimbal pitch setup starting ===";
|
||||
setProgress('?', 0, N, "starting");
|
||||
CalibReport report;
|
||||
|
||||
// 1. Preconditions.
|
||||
if (!imu_.connected()) {
|
||||
LOG_WARN << "calibration aborted: IMU not connected";
|
||||
LOG_WARN << "pitch setup aborted: IMU not connected (set [Features] enable_imu)";
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -158,12 +152,12 @@ void CalibrationRoutine::run() {
|
|||
MotorTelemetry t0 = motor_.telemetry();
|
||||
bool homed = t0.yaw.ready() && (!t0.pitch_present || t0.pitch.ready());
|
||||
if (!homed) {
|
||||
LOG_INFO << "calibration: gimbal not homed; homing first...";
|
||||
LOG_INFO << "pitch setup: gimbal not homed; homing first...";
|
||||
setProgress('?', 0, N, "homing");
|
||||
motor_.sendCommand("HOME");
|
||||
if (!waitHomed(motor_, params_.home_timeout_ms, cancel_)) {
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
LOG_WARN << "calibration aborted: homing did not reach READY";
|
||||
if (cancel_) { LOG_WARN << "pitch setup cancelled"; return; }
|
||||
LOG_WARN << "pitch setup aborted: homing did not reach READY";
|
||||
return;
|
||||
}
|
||||
t0 = motor_.telemetry();
|
||||
|
|
@ -171,7 +165,7 @@ void CalibrationRoutine::run() {
|
|||
const bool pitch_present = t0.pitch_present;
|
||||
|
||||
// Fetch soft limits from a fresh dump.
|
||||
LOG_INFO << "calibration: requesting soft limits (dump)...";
|
||||
LOG_INFO << "pitch setup: requesting soft limits (dump)...";
|
||||
motor_.sendCommand("DUMP");
|
||||
DumpData dump;
|
||||
for (int i = 0; i < 50 && !cancel_; ++i) { // up to ~5 s
|
||||
|
|
@ -180,13 +174,13 @@ void CalibrationRoutine::run() {
|
|||
msleep(100);
|
||||
}
|
||||
if (!dump.valid) {
|
||||
LOG_WARN << "calibration aborted: could not read soft limits";
|
||||
LOG_WARN << "pitch setup aborted: could not read soft limits";
|
||||
return;
|
||||
}
|
||||
|
||||
Geometry result = initial_;
|
||||
std::ostringstream log;
|
||||
log << "gimbal calibration\n==================\n";
|
||||
log << "gimbal pitch setup\n=================\n";
|
||||
|
||||
// Per-axis sweep span [a..b] (inset off the hard endstops) + a settle tolerance.
|
||||
auto axisSpan = [&](char axis, long& a, long& b, long& tol) -> bool {
|
||||
|
|
@ -206,16 +200,14 @@ void CalibrationRoutine::run() {
|
|||
// then least-squares fit counts vs degrees and record the result. Returns
|
||||
// false on cancel; on a degenerate fit it records a failed axis and returns true.
|
||||
auto sweepAxis = [&](char axis, AxisMap& map, long a, long b, long tol) -> bool {
|
||||
LOG_INFO << "calibrating " << (axis == 'Y' ? "YAW" : "PITCH")
|
||||
<< " over [" << a << ".." << b << "] counts in " << N << " steps";
|
||||
LOG_INFO << "measuring PITCH over [" << a << ".." << b << "] counts in " << N
|
||||
<< " steps";
|
||||
log << "\n[" << axis << "] sweep=[" << a << ".." << b << "] tol=" << tol
|
||||
<< "\n target_counts, imu_deg\n";
|
||||
|
||||
std::vector<std::pair<double, double>> pts; // (deg, counts)
|
||||
double prev_deg = 0.0;
|
||||
bool have_prev = false;
|
||||
for (int i = 0; i < N; ++i) {
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return false; }
|
||||
if (cancel_) { LOG_WARN << "pitch setup cancelled"; return false; }
|
||||
long target = a + (b - a) * i / (N - 1);
|
||||
setProgress(axis, i + 1, N, "moving");
|
||||
motor_.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||||
|
|
@ -225,12 +217,7 @@ void CalibrationRoutine::run() {
|
|||
<< " did not settle near " << target << " (continuing)";
|
||||
}
|
||||
setProgress(axis, i + 1, N, "dwelling");
|
||||
double deg = dwellAndMeasure(imu_, axis, params_.dwell_ms, params_.sample_ms, cancel_);
|
||||
// Unwrap against the previous sample so a sweep crossing 0/360 stays a
|
||||
// continuous line for the fit (no ±360 jump).
|
||||
if (have_prev) deg = unwrapNear(prev_deg, deg);
|
||||
prev_deg = deg;
|
||||
have_prev = true;
|
||||
const double deg = dwellAndMeasure(imu_, params_.dwell_ms, params_.sample_ms, cancel_);
|
||||
pts.emplace_back(deg, static_cast<double>(target));
|
||||
LOG_INFO << " " << (i + 1) << "/" << N << " counts=" << target
|
||||
<< " imu=" << deg << " deg";
|
||||
|
|
@ -246,89 +233,47 @@ void CalibrationRoutine::run() {
|
|||
return true;
|
||||
}
|
||||
const double old_cpd = map.counts_per_deg;
|
||||
const long old_zc = map.zero_count;
|
||||
const double old_dac = map.deg_at_center;
|
||||
// The fit gives counts as a function of degrees; the map is anchored on
|
||||
// the homed centre, so what we store is the angle that centre sits at.
|
||||
map.counts_per_deg = fit.slope;
|
||||
map.zero_count = static_cast<long>(std::lround(fit.intercept));
|
||||
map.deg_at_center =
|
||||
(static_cast<double>(map.center_count) - fit.intercept) / fit.slope;
|
||||
LOG_INFO << " fit: counts_per_deg " << old_cpd << " -> " << fit.slope
|
||||
<< ", zero_count " << old_zc << " -> " << map.zero_count
|
||||
<< ", deg_at_center " << old_dac << " -> " << map.deg_at_center
|
||||
<< " (R^2=" << fit.r2 << ")";
|
||||
log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << map.zero_count
|
||||
<< " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n";
|
||||
report.axes.push_back({axis, true, fit.slope, map.zero_count, fit.r2, fit.n});
|
||||
log << " fit: counts_per_deg=" << fit.slope << " deg_at_center=" << map.deg_at_center
|
||||
<< " R2=" << fit.r2 << " (was cpd=" << old_cpd << " dac=" << old_dac << ")\n";
|
||||
report.axes.push_back({axis, true, fit.slope, map.deg_at_center, fit.r2, fit.n});
|
||||
return true;
|
||||
};
|
||||
|
||||
// Resolve the yaw sweep span up front — we move to its first position before
|
||||
// the pitch sweep so the whole pitch calibration happens at that fixed yaw.
|
||||
long ya = 0, yb = 0, ytol = 0;
|
||||
if (!axisSpan('Y', ya, yb, ytol)) {
|
||||
LOG_WARN << "calibration aborted: no yaw soft limits in dump";
|
||||
// Sweep pitch across its inset travel, dwelling at each step to record the
|
||||
// gravity-referenced IMU pitch, and fit. Yaw is NOT touched: it is tied to
|
||||
// north by `gimbal heading setup` against real landmarks, which needs no IMU
|
||||
// and does not inherit the MTi's heading drift.
|
||||
if (!pitch_present) {
|
||||
LOG_WARN << "pitch setup aborted: this rig reports no pitch axis";
|
||||
return;
|
||||
}
|
||||
|
||||
// 3. Move yaw to its first sweep position and hold it there.
|
||||
LOG_INFO << "calibration: moving yaw to first position (counts=" << ya << ")";
|
||||
setProgress('Y', 1, N, "moving");
|
||||
motor_.sendCommand(std::string("MOVE Y ") + std::to_string(ya));
|
||||
waitSettle(motor_, 'Y', ya, ytol, params_.settle_timeout_ms, cancel_);
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
|
||||
// 4. Calibrate PITCH at this yaw position, then 5. move pitch to 0 deg.
|
||||
if (pitch_present) {
|
||||
long pa = 0, pb = 0, ptol = 0;
|
||||
if (axisSpan('P', pa, pb, ptol)) {
|
||||
if (!sweepAxis('P', result.pitch, pa, pb, ptol)) { return; } // cancelled
|
||||
const long pzero = result.pitch.toCounts(0.0);
|
||||
LOG_INFO << "calibration: moving pitch to 0 deg (counts=" << pzero << ")";
|
||||
setProgress('P', N, N, "pitch->0");
|
||||
motor_.sendCommand(std::string("MOVE P ") + std::to_string(pzero));
|
||||
waitSettle(motor_, 'P', pzero, ptol, params_.settle_timeout_ms, cancel_);
|
||||
} else {
|
||||
LOG_WARN << "calibration: no pitch soft limits in dump; skipping pitch";
|
||||
}
|
||||
long pa = 0, pb = 0, ptol = 0;
|
||||
if (!axisSpan('P', pa, pb, ptol)) {
|
||||
LOG_WARN << "pitch setup aborted: no pitch soft limits in dump";
|
||||
return;
|
||||
}
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
if (!sweepAxis('P', result.pitch, pa, pb, ptol)) return; // cancelled
|
||||
|
||||
// 6. Switch the IMU to a no-magnetometer XKF profile so its heading stops
|
||||
// chasing the (stepper-distorted) magnetic field. Selected from the device's
|
||||
// own available list; persists on the device. Best-effort — if none is
|
||||
// available we log and continue with the drift correction below.
|
||||
if (auto cfg = imu_.config()) {
|
||||
int nomag = pickNoMagProfile(cfg->available_profiles);
|
||||
if (nomag >= 0 && (!cfg->has_scenario || cfg->scenario_type != nomag)) {
|
||||
LOG_INFO << "calibration: switching IMU to no-mag XKF profile (type " << nomag << ")";
|
||||
setProgress('Y', 0, N, "no-mag profile");
|
||||
if (!imu_.setFilterProfile(nomag))
|
||||
LOG_WARN << "calibration: IMU did not confirm the no-mag profile (continuing)";
|
||||
} else if (nomag < 0) {
|
||||
LOG_WARN << "calibration: no no-mag XKF profile available on this device (continuing)";
|
||||
}
|
||||
}
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
// Park level, using the map just fitted - a visible confirmation that it is
|
||||
// right, and a sane place to leave the axis.
|
||||
const long plevel = result.pitch.toCounts(0.0);
|
||||
LOG_INFO << "pitch setup: moving to 0 deg / level (counts=" << plevel << ")";
|
||||
setProgress('P', N, N, "pitch->level");
|
||||
motor_.sendCommand(std::string("MOVE P ") + std::to_string(plevel));
|
||||
waitSettle(motor_, 'P', plevel, ptol, params_.settle_timeout_ms, cancel_);
|
||||
if (cancel_) { LOG_WARN << "pitch setup cancelled"; return; }
|
||||
|
||||
// 7. Hold still: correct yaw drift (no-rotation gyro-bias update) then reset
|
||||
// the IMU heading so this pose is yaw 0 — done right before the yaw sweep so
|
||||
// the drift accumulated during the long pitch sweep doesn't bias the fit.
|
||||
LOG_INFO << "calibration: correcting yaw drift (no-rotation " << params_.norotation_s
|
||||
<< " s, holding still)...";
|
||||
setProgress('Y', 0, N, "yaw drift correction");
|
||||
imu_.noRotation(params_.norotation_s);
|
||||
for (int waited = 0; waited < params_.norotation_s * 1000 + 500 && !cancel_; waited += 100)
|
||||
msleep(100);
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
LOG_INFO << "calibration: resetting IMU heading (this pose is now yaw 0)";
|
||||
setProgress('Y', 0, N, "yaw heading reset");
|
||||
imu_.headingReset();
|
||||
msleep(params_.reset_settle_ms);
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
|
||||
// 8. Calibrate YAW from this (now zero-heading) first position.
|
||||
if (!sweepAxis('Y', result.yaw, ya, yb, ytol)) { return; } // cancelled
|
||||
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
|
||||
std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());
|
||||
if (!path.empty()) LOG_INFO << "calibration log written: " << path;
|
||||
std::string path = paths::writeLogFile(paths::timestampedLogName("pitch_setup"), log.str());
|
||||
if (!path.empty()) LOG_INFO << "pitch setup log written: " << path;
|
||||
|
||||
report.valid = true;
|
||||
report.ts_ms = nowMs();
|
||||
|
|
@ -339,7 +284,7 @@ void CalibrationRoutine::run() {
|
|||
result_ = result;
|
||||
report_ = report;
|
||||
}
|
||||
LOG_INFO << "=== gimbal calibration complete (applied to session) ===";
|
||||
LOG_INFO << "=== gimbal pitch setup complete (applied to session) ===";
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -57,7 +57,7 @@ std::optional<AxisTelemetry> parseAxisSegment(const std::string& seg) {
|
|||
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
|
||||
default: break; // unknown letters ignored
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
288
src/ui/TuiUi.cpp
288
src/ui/TuiUi.cpp
|
|
@ -10,10 +10,12 @@
|
|||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
#include <ftxui/component/component.hpp>
|
||||
|
|
@ -152,7 +154,129 @@ Element keyHint(const std::string& key, const std::string& label) {
|
|||
|
||||
// Which overlay owns the body. Declared here so keyBar() can switch on it; the
|
||||
// render loop keeps the authoritative value.
|
||||
enum class Overlay { None, Help, Gimbal, Sensors, Cameras, Conn };
|
||||
enum class Overlay { None, Help, Gimbal, Sensors, Cameras, Conn, Heading };
|
||||
|
||||
|
||||
// ---- heading-setup wizard (Overlay::Heading) -------------------------------
|
||||
//
|
||||
// Ties encoder counts to true bearings by having the operator aim at landmarks
|
||||
// they can identify on a map. Deliberately does NOT need an IMU: not every tower
|
||||
// has one, and IMU yaw drifts too much to be a heading reference.
|
||||
//
|
||||
// The interaction lives here because arrow keys and typed input exist only in the
|
||||
// TUI; the fit itself is `gimbal heading fit`, an ordinary command, so it stays
|
||||
// testable and usable on a headless tower.
|
||||
|
||||
enum class JogPhase { Coarse, Fine, VeryFine };
|
||||
|
||||
// The finest step is 25 counts, not 1. ENC_CONST makes X_ENC advance in
|
||||
// 12.8-microstep quanta, so a smaller step moves the motor but records a position
|
||||
// the gimbal cannot reproduce - and 25 counts is already ~0.05 deg on yaw, half
|
||||
// the rig's pointing uncertainty, so it is never the limiting factor.
|
||||
constexpr long kVeryFineCounts = 25;
|
||||
constexpr double kCoarseTravelPct = 5.0;
|
||||
constexpr double kFineTravelPct = 0.5;
|
||||
|
||||
const char* jogPhaseName(JogPhase p) {
|
||||
switch (p) {
|
||||
case JogPhase::Coarse: return "coarse";
|
||||
case JogPhase::Fine: return "fine";
|
||||
case JogPhase::VeryFine: return "very fine";
|
||||
}
|
||||
return "?";
|
||||
}
|
||||
|
||||
// Step size in counts for a phase, given that axis's homed endstop-to-endstop
|
||||
// travel. Falls back to the very-fine step when travel is unknown (no dump yet),
|
||||
// which is slow but never wild.
|
||||
long jogStepCounts(JogPhase p, long travel_counts) {
|
||||
if (p == JogPhase::VeryFine || travel_counts <= 0) return kVeryFineCounts;
|
||||
const double pct = (p == JogPhase::Coarse) ? kCoarseTravelPct : kFineTravelPct;
|
||||
long step = static_cast<long>(pct / 100.0 * static_cast<double>(travel_counts));
|
||||
return step > kVeryFineCounts ? step : kVeryFineCounts;
|
||||
}
|
||||
|
||||
struct Sighting {
|
||||
long counts = 0;
|
||||
double bearing = 0.0;
|
||||
};
|
||||
|
||||
struct HeadingWizard {
|
||||
JogPhase phase = JogPhase::Coarse;
|
||||
bool entering = false; // typing a bearing for pending_counts
|
||||
long pending_counts = 0;
|
||||
std::vector<Sighting> sightings;
|
||||
std::string note; // last action / error, shown in the pane
|
||||
};
|
||||
|
||||
// Angular size of a step, when the yaw scale is known. Shown alongside the count
|
||||
// so the operator can judge the phase without doing the arithmetic.
|
||||
std::string jogStepLabel(JogPhase p, long travel_counts, double counts_per_deg) {
|
||||
const long step = jogStepCounts(p, travel_counts);
|
||||
std::string out = std::to_string(step) + " counts";
|
||||
if (std::fabs(counts_per_deg) > 1e-9)
|
||||
out += " (~" + num(std::fabs(step / counts_per_deg), 3) + " deg)";
|
||||
return out;
|
||||
}
|
||||
|
||||
Element headingWizardPanel(const UiSnapshot& s, const HeadingWizard& w, Component bearing_input) {
|
||||
const AxisView& y = s.gimbal.yaw;
|
||||
const long travel = y.has_limits ? std::labs(y.lim_pos - y.lim_neg) : 0;
|
||||
|
||||
Elements rows;
|
||||
rows.push_back(text("Aim at a landmark whose true bearing you know, then record it.") | bold);
|
||||
rows.push_back(text("Two landmarks minimum; the further apart, the better the fit.") | dim);
|
||||
rows.push_back(separator());
|
||||
|
||||
// Live position, in both the count the fit will use and the bearing the
|
||||
// current (possibly wrong) calibration believes.
|
||||
rows.push_back(hbox({text("position ") | dim,
|
||||
text(std::to_string(y.xenc) + " counts") | bold,
|
||||
text(" currently reads " + num(y.deg, 2) + " deg") | dim}));
|
||||
rows.push_back(hbox({text("step ") | dim,
|
||||
text(jogPhaseName(w.phase)) | bold | color(Color::Cyan),
|
||||
text(" " + jogStepLabel(w.phase, travel, y.counts_per_deg)) | dim}));
|
||||
if (!y.has_limits)
|
||||
rows.push_back(text(" no homed travel yet - steps fall back to the finest; "
|
||||
"home or press d in the gimbal view") | color(Color::Yellow));
|
||||
|
||||
rows.push_back(separator());
|
||||
if (w.sightings.empty()) {
|
||||
rows.push_back(text("no sightings recorded yet") | dim);
|
||||
} else {
|
||||
rows.push_back(text("sightings") | bold);
|
||||
for (size_t i = 0; i < w.sightings.size(); ++i)
|
||||
rows.push_back(text(" " + std::to_string(i + 1) + ". " +
|
||||
std::to_string(w.sightings[i].counts) + " counts -> " +
|
||||
num(w.sightings[i].bearing, 2) + " deg true"));
|
||||
}
|
||||
|
||||
rows.push_back(separator());
|
||||
if (w.entering) {
|
||||
rows.push_back(hbox({text("bearing of the landmark at " + std::to_string(w.pending_counts) +
|
||||
" counts (deg true): "),
|
||||
bearing_input->Render() | flex}));
|
||||
rows.push_back(text("Enter to record, Esc to discard this sighting") | dim);
|
||||
} else if (s.capture.snapshot_armed) {
|
||||
rows.push_back(text("snapshot armed - waiting for the gimbal to settle") |
|
||||
color(Color::Yellow));
|
||||
} else if (s.capture.has_last) {
|
||||
// The operator identifies the landmark from the image, which reaches them
|
||||
// through the ground station; the path is here so they can find the file.
|
||||
rows.push_back(hbox({text("last image ") | dim,
|
||||
text(s.capture.config.output_dir + "/" + s.capture.last_label + "/" +
|
||||
std::to_string(s.capture.last_ts_ms) + ".jxl")}));
|
||||
}
|
||||
if (!w.note.empty()) rows.push_back(text(w.note) | color(Color::Yellow));
|
||||
|
||||
rows.push_back(separator());
|
||||
rows.push_back(text(w.sightings.size() >= 2
|
||||
? "f to fit and apply"
|
||||
: "record at least two sightings before fitting") |
|
||||
(w.sightings.size() >= 2 ? color(Color::Green) : dim));
|
||||
|
||||
return vbox(std::move(rows)) | flex;
|
||||
}
|
||||
|
||||
// The bottom bar, contextual: the overview advertises everyday operations and
|
||||
// the overlay keys, and each overlay advertises what its own body responds to.
|
||||
|
|
@ -163,7 +287,7 @@ enum class Overlay { None, Help, Gimbal, Sensors, Cameras, Conn };
|
|||
//
|
||||
// flexbox (Wrap::Wrap by default) rather than hbox: the overview set is wider
|
||||
// than an 80-column terminal, and hbox silently truncated it mid-word ("q Qui").
|
||||
Element keyBar(Overlay overlay) {
|
||||
Element keyBar(Overlay overlay, bool manual) {
|
||||
static const char* kArrowsV = "\xE2\x86\x91\xE2\x86\x93";
|
||||
static const char* kArrows4 = "\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93";
|
||||
|
||||
|
|
@ -175,8 +299,14 @@ Element keyBar(Overlay overlay) {
|
|||
e.push_back(keyHint("h", "Home"));
|
||||
e.push_back(keyHint("r", "Refresh"));
|
||||
e.push_back(keyHint("g i c m", "Views"));
|
||||
if (manual) e.push_back(keyHint("H", "Heading"));
|
||||
e.push_back(keyHint("p", "Pause"));
|
||||
e.push_back(keyHint(kArrows4, "Nudge"));
|
||||
// Hand aiming is refused while a scan owns the axes, so stop
|
||||
// advertising it rather than inviting a keypress that only warns.
|
||||
if (manual) {
|
||||
e.push_back(keyHint(kArrows4, "Nudge"));
|
||||
e.push_back(keyHint("Spc", "Snap"));
|
||||
}
|
||||
e.push_back(keyHint("PgUp/Dn", "Log"));
|
||||
break;
|
||||
case Overlay::Help:
|
||||
|
|
@ -201,6 +331,16 @@ Element keyBar(Overlay overlay) {
|
|||
e.push_back(keyHint("PgUp/Dn", "Page"));
|
||||
e.push_back(keyHint("Home/End", "Ends"));
|
||||
break;
|
||||
case Overlay::Heading:
|
||||
// Replaces the common set entirely: start/stop are refused during
|
||||
// heading setup, so advertising them here would only mislead.
|
||||
e.clear();
|
||||
e.push_back(keyHint(kArrows4, "Aim"));
|
||||
e.push_back(keyHint("Tab", "Step size"));
|
||||
e.push_back(keyHint("Spc", "Snap"));
|
||||
e.push_back(keyHint("Enter", "Record"));
|
||||
e.push_back(keyHint("f", "Fit"));
|
||||
break;
|
||||
}
|
||||
|
||||
if (overlay != Overlay::None) e.push_back(keyHint("Esc", "Close"));
|
||||
|
|
@ -750,8 +890,7 @@ Element axisDumpDetail(const DumpAxis& ax) {
|
|||
hbox({text(std::string(1, ax.axis) + " ") | bold,
|
||||
text(ax.state_name) | color(Color::Green) | bold,
|
||||
text(" enabled=" + std::to_string(ax.enabled ? 1 : 0)) | dim,
|
||||
text(" enc=" + std::to_string(ax.has_encoder ? 1 : 0)) | dim,
|
||||
text(" eeprom=" + std::to_string(ax.eeprom_restored ? 1 : 0)) | dim}),
|
||||
text(" enc=" + std::to_string(ax.has_encoder ? 1 : 0)) | dim}),
|
||||
kvRow("limits", "[" + std::to_string(ax.lim_neg) + ".." + std::to_string(ax.lim_pos) + "]"),
|
||||
kvRow("hold_tgt", std::to_string(ax.hold_target)),
|
||||
reg("GCONF"), reg("CHOPCONF"), reg("XACTUAL"), reg("X_ENC"),
|
||||
|
|
@ -820,7 +959,7 @@ Element axisColumn(const std::string& title, const AxisView& live, const DumpDat
|
|||
col.push_back(text("CALIBRATION") | bold | color(Color::Magenta));
|
||||
if (calib_has && cal.ok) {
|
||||
col.push_back(kvRow("cnt/deg", fmt("%.3f", cal.counts_per_deg)));
|
||||
col.push_back(kvRow("zero", std::to_string(cal.zero_count)));
|
||||
col.push_back(kvRow("deg@ctr", fmt("%.3f", cal.deg_at_center)));
|
||||
col.push_back(kvRow("R2", fmt("%.4f", cal.r2), cal.r2 >= 0.99 ? Color::Green : Color::Yellow));
|
||||
col.push_back(kvRow("points", std::to_string(cal.n)));
|
||||
} else {
|
||||
|
|
@ -1123,9 +1262,20 @@ void TuiUi::uiLoop() {
|
|||
Overlay overlay = Overlay::None; // which takeover panel owns the main area
|
||||
int help_sel = 0;
|
||||
bool gimbal_dump_requested = false; // auto-pull a dump the first time
|
||||
bool calib_prompt = false; // a yes/no calib-save question is showing
|
||||
bool calib_prompt = false; // a yes/no calib-save question is showing
|
||||
// Mirrored out of the snapshot each render: CatchEvent runs on the same
|
||||
// thread but has no access to the renderer's local snapshot copy.
|
||||
bool manual_mode = true;
|
||||
bool exit_prompt = false; // 'q' asked; awaiting quit/background/cancel
|
||||
|
||||
// Heading-setup wizard. Its own text field, so the command line keeps its
|
||||
// buffer and history semantics untouched.
|
||||
HeadingWizard wiz;
|
||||
std::string bearing_buffer;
|
||||
long wiz_yaw_counts = 0; // live yaw position, mirrored for CatchEvent
|
||||
long wiz_yaw_travel = 0;
|
||||
long wiz_pitch_travel = 0;
|
||||
|
||||
// Per-pane scroll positions. UI-thread-only view state (the renderer lambda
|
||||
// and CatchEvent below both run on this thread), so unlike screen_/paused_
|
||||
// these need no synchronisation and stay locals rather than TuiUi members.
|
||||
|
|
@ -1135,8 +1285,15 @@ void TuiUi::uiLoop() {
|
|||
size_t log_dropped_at_freeze = 0; // buffer evictions when scrollback was entered
|
||||
|
||||
auto input = Input(&cmd_buffer, "type a command, Enter to run, Esc to cancel");
|
||||
auto bearing_input = Input(&bearing_buffer, "e.g. 137.4");
|
||||
// Only one text field is ever live, so a Tab container (rather than a
|
||||
// Vertical) keeps focus unambiguous: index 0 = command line, 1 = the wizard's
|
||||
// bearing field. Renderer's child is fixed at construction, hence the
|
||||
// container rather than swapping the Input.
|
||||
int focused_input = 0;
|
||||
auto inputs = Container::Tab({input, bearing_input}, &focused_input);
|
||||
|
||||
auto renderer = Renderer(input, [&] {
|
||||
auto renderer = Renderer(inputs, [&] {
|
||||
UiSnapshot s = snapshot_ ? snapshot_() : UiSnapshot{};
|
||||
size_t log_dropped_now = 0;
|
||||
{
|
||||
|
|
@ -1152,6 +1309,12 @@ void TuiUi::uiLoop() {
|
|||
log_dropped_at_freeze = log_dropped_now;
|
||||
}
|
||||
calib_prompt = !s.activity.prompt.empty();
|
||||
manual_mode = s.capture.manual;
|
||||
wiz_yaw_counts = s.gimbal.yaw.xenc;
|
||||
wiz_yaw_travel = s.gimbal.yaw.has_limits ? std::labs(s.gimbal.yaw.lim_pos - s.gimbal.yaw.lim_neg) : 0;
|
||||
wiz_pitch_travel =
|
||||
s.gimbal.pitch.has_limits ? std::labs(s.gimbal.pitch.lim_pos - s.gimbal.pitch.lim_neg) : 0;
|
||||
focused_input = wiz.entering ? 1 : 0;
|
||||
gimbal_overlay_open_.store(overlay == Overlay::Gimbal); // refreshLoop polls a dump while open
|
||||
|
||||
std::string mode = s.header.live ? "LIVE" : "MOCK";
|
||||
|
|
@ -1183,7 +1346,7 @@ void TuiUi::uiLoop() {
|
|||
Element bottom = command_mode
|
||||
? hbox({text(" : ") | inverted, input->Render() | flex}) | border
|
||||
: exit_prompt ? exitPrompt(backgroundAvailable(std::getenv("TMUX")))
|
||||
: keyBar(overlay);
|
||||
: keyBar(overlay, s.capture.manual);
|
||||
|
||||
// An open overlay takes over the whole body (the dashboard panels are
|
||||
// hidden) so it has the full height — otherwise tall content like the
|
||||
|
|
@ -1203,6 +1366,9 @@ void TuiUi::uiLoop() {
|
|||
return vbox({header, separator(), cameraDetailPanel(s.capture, cam_scroll) | flex, bottom});
|
||||
case Overlay::Conn:
|
||||
return vbox({header, separator(), connDetailPanel(s.conn, conn_scroll) | flex, bottom});
|
||||
case Overlay::Heading:
|
||||
return vbox({header, separator(),
|
||||
headingWizardPanel(s, wiz, bearing_input) | flex, bottom});
|
||||
default: {
|
||||
// Activity strip sits between the log and the key bar; shown only
|
||||
// once a special op has run or is running, else it costs no space.
|
||||
|
|
@ -1274,6 +1440,90 @@ void TuiUi::uiLoop() {
|
|||
return true; // swallow everything else
|
||||
}
|
||||
|
||||
// The heading wizard owns the keyboard while it is up, like the exit
|
||||
// prompt: a stray key must not reach the dashboard behind it and move the
|
||||
// gimbal between a sighting and the bearing being typed for it.
|
||||
if (overlay == Overlay::Heading) {
|
||||
auto jog = [&](const char* axis, long travel, long sign) {
|
||||
if (!sink_) return;
|
||||
const long step = jogStepCounts(wiz.phase, travel) * sign;
|
||||
sink_(std::string("gimbal jog ") + axis + " " + std::to_string(step));
|
||||
};
|
||||
|
||||
if (wiz.entering) {
|
||||
if (e == Event::Return) {
|
||||
try {
|
||||
size_t used = 0;
|
||||
const double bearing = std::stod(bearing_buffer, &used);
|
||||
if (used != bearing_buffer.size()) throw std::invalid_argument("trailing");
|
||||
wiz.sightings.push_back({wiz.pending_counts, bearing});
|
||||
wiz.note = "recorded " + num(bearing, 2) + " deg at " +
|
||||
std::to_string(wiz.pending_counts) + " counts";
|
||||
wiz.entering = false;
|
||||
bearing_buffer.clear();
|
||||
} catch (const std::exception&) {
|
||||
wiz.note = "'" + bearing_buffer + "' is not a bearing in degrees";
|
||||
}
|
||||
return true;
|
||||
}
|
||||
if (e == Event::Escape) {
|
||||
wiz.entering = false;
|
||||
bearing_buffer.clear();
|
||||
wiz.note = "sighting discarded";
|
||||
return true;
|
||||
}
|
||||
return false; // let the Input edit the buffer
|
||||
}
|
||||
|
||||
if (e == Event::ArrowLeft) { jog("yaw", wiz_yaw_travel, -1); return true; }
|
||||
if (e == Event::ArrowRight) { jog("yaw", wiz_yaw_travel, +1); return true; }
|
||||
// Pitch too: the landmark has to be in frame vertically before its
|
||||
// bearing means anything.
|
||||
if (e == Event::ArrowUp) { jog("pitch", wiz_pitch_travel, +1); return true; }
|
||||
if (e == Event::ArrowDown) { jog("pitch", wiz_pitch_travel, -1); return true; }
|
||||
if (e == Event::Tab) {
|
||||
wiz.phase = wiz.phase == JogPhase::Coarse ? JogPhase::Fine
|
||||
: wiz.phase == JogPhase::Fine ? JogPhase::VeryFine
|
||||
: JogPhase::Coarse;
|
||||
return true;
|
||||
}
|
||||
if (e == Event::Return) {
|
||||
// Freeze the position now: the operator is about to look away to
|
||||
// read a bearing off a map, and the field must belong to where the
|
||||
// gimbal is pointing at this moment.
|
||||
wiz.pending_counts = wiz_yaw_counts;
|
||||
wiz.entering = true;
|
||||
wiz.note.clear();
|
||||
bearing_buffer.clear();
|
||||
return true;
|
||||
}
|
||||
if (e == Event::Escape) {
|
||||
overlay = Overlay::None;
|
||||
wiz = {};
|
||||
bearing_buffer.clear();
|
||||
if (sink_) sink_("gimbal heading cancel");
|
||||
return true;
|
||||
}
|
||||
if (e.is_character()) {
|
||||
const std::string& k = e.character();
|
||||
if (k == " ") { if (sink_) sink_("snapshot"); return true; }
|
||||
if (k == "f" || k == "F") {
|
||||
if (wiz.sightings.size() < 2) {
|
||||
wiz.note = "need at least two sightings before fitting";
|
||||
return true;
|
||||
}
|
||||
std::string cmd = "gimbal heading fit";
|
||||
for (const auto& sg : wiz.sightings)
|
||||
cmd += " " + std::to_string(sg.counts) + ":" + num(sg.bearing, 4);
|
||||
if (sink_) sink_(cmd);
|
||||
overlay = Overlay::None;
|
||||
wiz = {};
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return true; // swallow everything else
|
||||
}
|
||||
|
||||
const int n = static_cast<int>(helpCatalog().size());
|
||||
if (overlay == Overlay::Help) { // help pane navigation
|
||||
// Selecting a different section replaces the body wholesale, so the
|
||||
|
|
@ -1335,8 +1585,10 @@ void TuiUi::uiLoop() {
|
|||
}
|
||||
}
|
||||
// Arrow keys nudge the gimbal in steps (only when no overlay is open):
|
||||
// Left/Right = yaw -/+5%, Up/Down = pitch -/+10% of travel.
|
||||
if (overlay == Overlay::None && sink_) {
|
||||
// Left/Right = yaw -/+5%, Up/Down = pitch -/+10% of travel. Only in
|
||||
// manual mode: during a scan the app refuses these anyway, and a silent
|
||||
// no-op beats a warning per keypress from someone leaning on an arrow.
|
||||
if (overlay == Overlay::None && manual_mode && sink_) {
|
||||
if (e == Event::ArrowLeft) { sink_("gimbal nudge yaw -5"); return true; }
|
||||
if (e == Event::ArrowRight) { sink_("gimbal nudge yaw 5"); return true; }
|
||||
if (e == Event::ArrowUp) { sink_("gimbal nudge pitch -10"); return true; }
|
||||
|
|
@ -1400,6 +1652,20 @@ void TuiUi::uiLoop() {
|
|||
if (c == "h") { if (sink_) sink_("gimbal home"); return true; }
|
||||
if (c == "r") { if (sink_) sink_("refresh"); return true; } // IMU config + dump
|
||||
if (c == ":") { command_mode = true; return true; }
|
||||
// Heading setup: only from the dashboard, and only in manual mode - the
|
||||
// app refuses to arm it during a scan, so opening the pane would strand
|
||||
// the operator in a wizard that cannot record anything.
|
||||
if (c == "H") {
|
||||
if (!manual_mode) return true;
|
||||
overlay = Overlay::Heading;
|
||||
wiz = {};
|
||||
bearing_buffer.clear();
|
||||
if (sink_) sink_("gimbal heading setup");
|
||||
return true;
|
||||
}
|
||||
// Spacebar: one image now, through the same pipeline a scan waypoint uses.
|
||||
// The app refuses it during a scan; the bar just stops advertising it.
|
||||
if (c == " ") { if (sink_) sink_("snapshot"); return true; }
|
||||
return false;
|
||||
});
|
||||
|
||||
|
|
|
|||
|
|
@ -41,7 +41,8 @@ add_executable(fgc_tests
|
|||
test_helptext.cpp
|
||||
test_diagparser.cpp
|
||||
test_calibration.cpp
|
||||
test_calibroutine.cpp
|
||||
test_headingfit.cpp
|
||||
test_pitchsetup.cpp
|
||||
test_testreport.cpp
|
||||
test_hostmetrics.cpp
|
||||
test_testrunner.cpp
|
||||
|
|
|
|||
|
|
@ -236,16 +236,16 @@ TEST_CASE("updateIniSectionKeys replaces in-section keys, leaving others intact"
|
|||
"[Motor]\n"
|
||||
"; calibrate me\n"
|
||||
"yaw_counts_per_deg = 983.33\n"
|
||||
"yaw_zero_count = 500000\n"
|
||||
"yaw_center_count = 500000\n"
|
||||
"yaw_min_deg = -90\n"
|
||||
"[Scan]\n"
|
||||
"yaw_min_deg = -90\n"; // same key name, different section — must NOT change
|
||||
|
||||
auto out = updateIniSectionKeys(in, "Motor",
|
||||
{{"yaw_counts_per_deg", "1000"}, {"yaw_zero_count", "12345"}});
|
||||
{{"yaw_counts_per_deg", "1000"}, {"yaw_center_count", "12345"}});
|
||||
|
||||
CHECK(out.find("yaw_counts_per_deg = 1000") != std::string::npos);
|
||||
CHECK(out.find("yaw_zero_count = 12345") != std::string::npos);
|
||||
CHECK(out.find("yaw_center_count = 12345") != std::string::npos);
|
||||
CHECK(out.find("yaw_counts_per_deg = 983.33") == std::string::npos); // replaced
|
||||
CHECK(out.find("; calibrate me") != std::string::npos); // comment kept
|
||||
CHECK(out.find("tower_name = Foo") != std::string::npos); // other section kept
|
||||
|
|
@ -255,49 +255,87 @@ TEST_CASE("updateIniSectionKeys replaces in-section keys, leaving others intact"
|
|||
|
||||
TEST_CASE("updateIniSectionKeys appends keys missing from the section") {
|
||||
const std::string in = "[Motor]\nyaw_counts_per_deg = 1\n";
|
||||
auto out = updateIniSectionKeys(in, "Motor", {{"pitch_zero_count", "7"}});
|
||||
auto out = updateIniSectionKeys(in, "Motor", {{"pitch_center_count", "7"}});
|
||||
// re-parsing must see the appended key under [Motor]
|
||||
CHECK(out.find("pitch_zero_count = 7") != std::string::npos);
|
||||
CHECK(out.find("pitch_center_count = 7") != std::string::npos);
|
||||
}
|
||||
|
||||
TEST_CASE("saveMotorCalibration round-trips the full per-axis map") {
|
||||
TEST_CASE("saveMotorGeometry round-trips the full per-axis map") {
|
||||
// Write a starting config, save a calibrated Geometry into it, reload, and
|
||||
// confirm every [Motor] parameter (incl. min/max_deg) survives, while another
|
||||
// section is left untouched.
|
||||
// confirm every [Motor] parameter survives, while another section is left
|
||||
// untouched. min_deg/max_deg are deliberately NOT persisted - they come from
|
||||
// homing - so they must come back at their defaults, not at the saved values.
|
||||
const std::string path = "test_calib_roundtrip.ini";
|
||||
{
|
||||
std::ofstream f(path, std::ios::trunc);
|
||||
f << "[General]\ntower_name = rig7\n\n"
|
||||
"[Motor]\n"
|
||||
"yaw_counts_per_deg = 1.0\n"
|
||||
"yaw_zero_count = 0\n"
|
||||
"yaw_min_deg = -90\n"
|
||||
"yaw_max_deg = 90\n"
|
||||
"yaw_center_count = 500000\n"
|
||||
"yaw_heading_at_center = 0\n"
|
||||
"pitch_counts_per_deg = 1.0\n"
|
||||
"pitch_zero_count = 0\n"
|
||||
"pitch_min_deg = 0\n"
|
||||
"pitch_max_deg = 60\n";
|
||||
"pitch_center_count = 500000\n"
|
||||
"pitch_angle_at_center = 0\n";
|
||||
}
|
||||
|
||||
Geometry geo;
|
||||
geo.yaw = {983.33, 500123, -88.0, 92.0};
|
||||
geo.pitch = {8333.33, -4200, -2.0, 61.5};
|
||||
REQUIRE(saveMotorCalibration(path, geo));
|
||||
geo.yaw = {983.33, 500123, 137.25, -88.0, 92.0};
|
||||
geo.pitch = {8333.33, 499000, 10.5, -2.0, 61.5};
|
||||
REQUIRE(saveMotorGeometry(path, geo, 4000, 6000));
|
||||
|
||||
AppConfig cfg = ConfigLoader::loadFromFile(path);
|
||||
CHECK(cfg.general.tower_name == "rig7"); // unrelated section preserved
|
||||
CHECK(cfg.geometry.yaw.counts_per_deg == doctest::Approx(983.33));
|
||||
CHECK(cfg.geometry.yaw.zero_count == 500123);
|
||||
CHECK(cfg.geometry.yaw.min_deg == doctest::Approx(-88.0));
|
||||
CHECK(cfg.geometry.yaw.max_deg == doctest::Approx(92.0));
|
||||
CHECK(cfg.geometry.yaw.center_count == 500123);
|
||||
CHECK(cfg.geometry.yaw.deg_at_center == doctest::Approx(137.25));
|
||||
CHECK(cfg.geometry.pitch.counts_per_deg == doctest::Approx(8333.33));
|
||||
CHECK(cfg.geometry.pitch.zero_count == -4200);
|
||||
CHECK(cfg.geometry.pitch.min_deg == doctest::Approx(-2.0));
|
||||
CHECK(cfg.geometry.pitch.max_deg == doctest::Approx(61.5));
|
||||
CHECK(cfg.geometry.pitch.center_count == 499000);
|
||||
CHECK(cfg.geometry.pitch.deg_at_center == doctest::Approx(10.5));
|
||||
CHECK(cfg.yaw_soft_margin_counts == 4000);
|
||||
CHECK(cfg.pitch_soft_margin_counts == 6000);
|
||||
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST_CASE("the [Motor] loader rejects values that would silently misaim the gimbal") {
|
||||
auto base = [] {
|
||||
return std::map<std::string, std::string>{
|
||||
{"Motor.yaw_counts_per_deg", "983.33"}, {"Motor.yaw_center_count", "500000"},
|
||||
{"Motor.pitch_counts_per_deg", "8333.33"}, {"Motor.pitch_center_count", "500000"}};
|
||||
};
|
||||
CHECK_NOTHROW(ConfigLoader::fromMap(base()));
|
||||
|
||||
// A zero scale maps every degree to the centre: it would look calibrated.
|
||||
auto zero_scale = base();
|
||||
zero_scale["Motor.yaw_counts_per_deg"] = "0";
|
||||
CHECK_THROWS_AS(ConfigLoader::fromMap(zero_scale), std::runtime_error);
|
||||
|
||||
// The centre count exists to keep the working range positive.
|
||||
auto neg_centre = base();
|
||||
neg_centre["Motor.pitch_center_count"] = "-1";
|
||||
CHECK_THROWS_AS(ConfigLoader::fromMap(neg_centre), std::runtime_error);
|
||||
|
||||
auto neg_margin = base();
|
||||
neg_margin["Motor.yaw_soft_margin_counts"] = "-100";
|
||||
CHECK_THROWS_AS(ConfigLoader::fromMap(neg_margin), std::runtime_error);
|
||||
}
|
||||
|
||||
TEST_CASE("keys from the old [Motor] layout are ignored, not silently honoured") {
|
||||
// A config still carrying yaw_zero_count must load (so an upgrade is not a
|
||||
// hard stop) but must not pick the stale origin up from anywhere.
|
||||
std::map<std::string, std::string> kv{
|
||||
{"Motor.yaw_counts_per_deg", "983.33"}, {"Motor.yaw_center_count", "500000"},
|
||||
{"Motor.yaw_heading_at_center", "42"}, {"Motor.yaw_zero_count", "851592"},
|
||||
{"Motor.yaw_min_deg", "0"}, {"Motor.yaw_max_deg", "319"},
|
||||
{"Motor.yaw_home_zero", "high"}};
|
||||
AppConfig cfg = ConfigLoader::fromMap(kv);
|
||||
CHECK(cfg.geometry.yaw.center_count == 500000);
|
||||
CHECK(cfg.geometry.yaw.deg_at_center == doctest::Approx(42.0));
|
||||
// The removed range keys must not have leaked into the derived window.
|
||||
CHECK(cfg.geometry.yaw.min_deg == doctest::Approx(-100000.0));
|
||||
CHECK(cfg.geometry.yaw.max_deg == doctest::Approx(100000.0));
|
||||
}
|
||||
|
||||
TEST_CASE("loadFromFile anchors a relative profile_dir to the config file's directory") {
|
||||
// The config is found through a search path that has nothing to do with the
|
||||
// working directory, so a relative profile_dir must resolve next to the
|
||||
|
|
|
|||
|
|
@ -14,15 +14,15 @@ namespace {
|
|||
// TMC register line, bracketed by DUMP BEGIN / DUMP END.
|
||||
const char* kDump =
|
||||
"DUMP BEGIN build=11dd3ce-dirty uptime=11740067 mcusr=0x01 free_ram=1852\n"
|
||||
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-82919 lim_pos=98687 hold_target=-90 "
|
||||
"speed=50000 eeprom_restored=0 soft_margin=5000 hold_corr=7 hold_peak=430 hold_cumul=2810 "
|
||||
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=411500 lim_pos=588500 hold_target=499910 "
|
||||
"speed=50000 center=500000 soft_margin=5000 hold_corr=7 hold_peak=430 hold_cumul=2810 "
|
||||
"has_encoder=1\n"
|
||||
"DUMP Y TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
|
||||
"RAMPMODE=0x00000000 XACTUAL=0xFFFFFFA6 VACTUAL=0x00000000 XTARGET=0xFFFFFFA6 "
|
||||
"SW_MODE=0x000008A0 RAMP_STAT=0x00001680 X_ENC=0xFFFFFF7F ENC_STATUS=0x00000002 "
|
||||
"CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00000011 PWM_AUTO=0x003F0039\n"
|
||||
"DUMP P state=3 hsub=10 enabled=1 lim_neg=495183 lim_pos=1069042 hold_target=500183 "
|
||||
"speed=150000 eeprom_restored=0 has_encoder=1\n"
|
||||
"DUMP P state=3 hsub=10 enabled=1 lim_neg=213070 lim_pos=786930 hold_target=500183 "
|
||||
"speed=150000 center=500000 has_encoder=1\n"
|
||||
"DUMP P TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
|
||||
"RAMPMODE=0x00000000 XACTUAL=0x0007A1C8 VACTUAL=0x00000000 XTARGET=0x0007A1C8 "
|
||||
"SW_MODE=0x000008A3 RAMP_STAT=0x00001688 X_ENC=0x0007A231 ENC_STATUS=0x00000002 "
|
||||
|
|
@ -59,10 +59,12 @@ TEST_CASE("parseDump decodes the per-axis state line") {
|
|||
CHECK(y->state_name == "READY"); // state=3
|
||||
CHECK(y->enabled);
|
||||
CHECK(y->has_encoder);
|
||||
CHECK_FALSE(y->eeprom_restored);
|
||||
CHECK(y->lim_neg == -82919);
|
||||
CHECK(y->lim_pos == 98687);
|
||||
CHECK(y->hold_target == -90);
|
||||
CHECK(y->lim_neg == 411500);
|
||||
CHECK(y->lim_pos == 588500);
|
||||
CHECK(y->hold_target == 499910);
|
||||
// The anchor the firmware homed onto; limits straddle it symmetrically.
|
||||
CHECK(y->center_count == 500000);
|
||||
CHECK((y->lim_neg + y->lim_pos) / 2 == y->center_count);
|
||||
CHECK(y->speed == 50000);
|
||||
CHECK(y->hsub == 10);
|
||||
CHECK(y->soft_margin == 5000);
|
||||
|
|
@ -72,8 +74,8 @@ TEST_CASE("parseDump decodes the per-axis state line") {
|
|||
|
||||
const DumpAxis* p = axis(d, 'P');
|
||||
REQUIRE(p != nullptr);
|
||||
CHECK(p->lim_neg == 495183);
|
||||
CHECK(p->lim_pos == 1069042);
|
||||
CHECK(p->lim_neg == 213070);
|
||||
CHECK(p->lim_pos == 786930);
|
||||
}
|
||||
|
||||
TEST_CASE("parseDump keeps all 16 TMC registers (regression: TMC-line whitespace)") {
|
||||
|
|
|
|||
|
|
@ -4,8 +4,10 @@
|
|||
|
||||
using namespace fgc;
|
||||
|
||||
TEST_CASE("AxisMap converts degrees and counts with an offset") {
|
||||
AxisMap m{983.33, 500000, -90.0, 90.0};
|
||||
// Fields in order: counts_per_deg, center_count, deg_at_center, min_deg, max_deg.
|
||||
|
||||
TEST_CASE("AxisMap converts degrees and counts about the anchored centre") {
|
||||
AxisMap m{983.33, 500000, 0.0, -90.0, 90.0};
|
||||
|
||||
CHECK(m.toCounts(0.0) == 500000);
|
||||
CHECK(m.toCounts(90.0) == 588500); // lround(500000 + 90*983.33)
|
||||
|
|
@ -13,20 +15,94 @@ TEST_CASE("AxisMap converts degrees and counts with an offset") {
|
|||
CHECK(m.toDeg(m.toCounts(45.0)) == doctest::Approx(45.0).epsilon(0.001));
|
||||
}
|
||||
|
||||
TEST_CASE("a non-zero deg_at_center shifts the whole map") {
|
||||
// The centre points at 137.4 deg true; everything else follows from that.
|
||||
AxisMap m{-529.8, 500000, 137.4, 137.4 - 160.0, 137.4 + 160.0};
|
||||
|
||||
CHECK(m.toCounts(137.4) == 500000);
|
||||
CHECK(m.toDeg(500000) == doctest::Approx(137.4));
|
||||
// Negative counts_per_deg: a higher heading is a lower count.
|
||||
CHECK(m.toCounts(147.4) < 500000);
|
||||
CHECK(m.toDeg(m.toCounts(200.0)) == doctest::Approx(200.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
|
||||
AxisMap m{10.0, 0, 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};
|
||||
AxisMap m{-10.0, 1000, 0.0, -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));
|
||||
// Degrades to "we are at the centre", not to an unrelated zero.
|
||||
AxisMap m{0.0, 0, 42.0, -90.0, 90.0};
|
||||
CHECK(m.toDeg(12345) == doctest::Approx(42.0));
|
||||
}
|
||||
|
||||
TEST_CASE("wrap360 and wrap180 normalise at and across the boundaries") {
|
||||
CHECK(wrap360(0.0) == doctest::Approx(0.0));
|
||||
CHECK(wrap360(360.0) == doctest::Approx(0.0));
|
||||
CHECK(wrap360(361.0) == doctest::Approx(1.0));
|
||||
CHECK(wrap360(-1.0) == doctest::Approx(359.0));
|
||||
CHECK(wrap360(-361.0) == doctest::Approx(359.0));
|
||||
CHECK(wrap360(180.0) == doctest::Approx(180.0));
|
||||
// A tiny negative must land at 0, never at exactly 360.
|
||||
CHECK(wrap360(-1e-15) < 360.0);
|
||||
|
||||
CHECK(wrap180(0.0) == doctest::Approx(0.0));
|
||||
CHECK(wrap180(179.0) == doctest::Approx(179.0));
|
||||
CHECK(wrap180(180.0) == doctest::Approx(-180.0));
|
||||
CHECK(wrap180(181.0) == doctest::Approx(-179.0));
|
||||
CHECK(wrap180(-179.0) == doctest::Approx(-179.0));
|
||||
CHECK(wrap180(359.0) == doctest::Approx(-1.0));
|
||||
}
|
||||
|
||||
TEST_CASE("headingDeg reports a compass bearing regardless of the linear degree") {
|
||||
// Centre at 350 deg true; +20 deg of travel crosses north.
|
||||
AxisMap m{100.0, 500000, 350.0, 350.0 - 160.0, 350.0 + 160.0};
|
||||
|
||||
CHECK(m.headingDeg(500000) == doctest::Approx(350.0));
|
||||
// Linear 370 deg is the compass bearing 10 deg.
|
||||
CHECK(m.headingDeg(m.toCounts(370.0)) == doctest::Approx(10.0));
|
||||
}
|
||||
|
||||
TEST_CASE("headingToCounts reaches across north") {
|
||||
AxisMap m{100.0, 500000, 350.0, 350.0 - 160.0, 350.0 + 160.0};
|
||||
|
||||
// 10 deg true is 20 deg past the centre the short way, well inside travel.
|
||||
auto c = m.headingToCounts(10.0);
|
||||
REQUIRE(c.has_value());
|
||||
CHECK(*c == 502000);
|
||||
CHECK(m.headingDeg(*c) == doctest::Approx(10.0));
|
||||
|
||||
// And the other way, back past north.
|
||||
auto d = m.headingToCounts(330.0);
|
||||
REQUIRE(d.has_value());
|
||||
CHECK(*d == 498000);
|
||||
}
|
||||
|
||||
TEST_CASE("headingToCounts refuses the dead arc instead of clamping to a limit") {
|
||||
// 320 deg of travel centred on 0 deg true: reachable is 200..360..160.
|
||||
AxisMap m{100.0, 500000, 0.0, -160.0, 160.0};
|
||||
|
||||
CHECK(m.headingToCounts(90.0).has_value());
|
||||
CHECK(m.headingToCounts(270.0).has_value()); // == -90, inside
|
||||
CHECK(m.headingToCounts(160.0).has_value()); // exactly on the limit
|
||||
CHECK_FALSE(m.headingToCounts(180.0).has_value()); // dead centre of the gap
|
||||
CHECK_FALSE(m.headingToCounts(170.0).has_value());
|
||||
CHECK_FALSE(m.headingToCounts(190.0).has_value());
|
||||
|
||||
// The point of returning nullopt: 180 must NOT come back as the 160 limit.
|
||||
CHECK(m.headingToCounts(180.0) != m.headingToCounts(160.0));
|
||||
}
|
||||
|
||||
TEST_CASE("headingToCounts refuses a degenerate scale") {
|
||||
AxisMap m{0.0, 500000, 0.0, -160.0, 160.0};
|
||||
CHECK_FALSE(m.headingToCounts(90.0).has_value());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,161 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/Geometry.h"
|
||||
#include "fgc/HeadingFit.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
namespace {
|
||||
|
||||
// A rig whose yaw runs at -529.8 counts/deg (the direction sense is flipped) with
|
||||
// the homed centre at count 500000. Build the sighting a landmark at `bearing`
|
||||
// would produce on a gimbal already calibrated to `cpd` / `at_center`.
|
||||
long countsFor(double bearing, double cpd, double at_center, long center = 500000) {
|
||||
return center + static_cast<long>(std::lround((bearing - at_center) * cpd));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("two sightings recover the map exactly") {
|
||||
const double cpd = -529.8, at_center = 137.4;
|
||||
std::vector<HeadingSighting> s{
|
||||
{countsFor(90.0, cpd, at_center), 90.0},
|
||||
{countsFor(200.0, cpd, at_center), 200.0},
|
||||
};
|
||||
|
||||
auto r = fitHeading(s, 500000);
|
||||
REQUIRE(r.ok);
|
||||
CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(1e-4));
|
||||
CHECK(r.deg_at_center == doctest::Approx(at_center).epsilon(1e-4));
|
||||
CHECK(r.n == 2);
|
||||
CHECK(r.r2 == doctest::Approx(1.0));
|
||||
}
|
||||
|
||||
TEST_CASE("sighting order does not matter") {
|
||||
const double cpd = 983.33, at_center = 40.0;
|
||||
std::vector<HeadingSighting> forward{
|
||||
{countsFor(10.0, cpd, at_center), 10.0},
|
||||
{countsFor(70.0, cpd, at_center), 70.0},
|
||||
{countsFor(130.0, cpd, at_center), 130.0},
|
||||
};
|
||||
std::vector<HeadingSighting> jumbled{forward[2], forward[0], forward[1]};
|
||||
|
||||
auto a = fitHeading(forward, 500000);
|
||||
auto b = fitHeading(jumbled, 500000);
|
||||
REQUIRE(a.ok);
|
||||
REQUIRE(b.ok);
|
||||
CHECK(a.counts_per_deg == doctest::Approx(b.counts_per_deg));
|
||||
CHECK(a.deg_at_center == doctest::Approx(b.deg_at_center));
|
||||
}
|
||||
|
||||
TEST_CASE("a set straddling north fits as one line, not two") {
|
||||
// Centre points at 350 deg; landmarks at 320 and 30 sit either side of north.
|
||||
// Without unwrapping, 30 would look like it is 290 deg *below* 320 and the
|
||||
// fitted scale would come out with the wrong sign and magnitude.
|
||||
const double cpd = 100.0, at_center = 350.0;
|
||||
std::vector<HeadingSighting> s{
|
||||
{countsFor(320.0, cpd, at_center), 320.0},
|
||||
{countsFor(390.0, cpd, at_center), 30.0}, // 390 == 30 deg true
|
||||
};
|
||||
|
||||
auto r = fitHeading(s, 500000);
|
||||
REQUIRE(r.ok);
|
||||
CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(1e-6));
|
||||
CHECK(r.deg_at_center == doctest::Approx(350.0).epsilon(1e-6));
|
||||
CHECK(r.span_deg == doctest::Approx(70.0));
|
||||
}
|
||||
|
||||
TEST_CASE("more sightings are least-squares fitted and report a usable R^2") {
|
||||
const double cpd = 983.33, at_center = 40.0;
|
||||
std::vector<HeadingSighting> s;
|
||||
// A couple of counts of aiming slop per sighting, alternating.
|
||||
const long slop[] = {40, -35, 30, -45, 25};
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
const double bearing = 10.0 + 40.0 * i;
|
||||
s.push_back({countsFor(bearing, cpd, at_center) + slop[i], bearing});
|
||||
}
|
||||
|
||||
auto r = fitHeading(s, 500000);
|
||||
REQUIRE(r.ok);
|
||||
CHECK(r.n == 5);
|
||||
CHECK(r.counts_per_deg == doctest::Approx(cpd).epsilon(0.01));
|
||||
CHECK(r.deg_at_center == doctest::Approx(at_center).epsilon(0.01));
|
||||
CHECK(r.r2 > 0.999); // slop is tiny against a 160 deg baseline
|
||||
}
|
||||
|
||||
TEST_CASE("a single sighting cannot determine a scale") {
|
||||
std::vector<HeadingSighting> s{{500000, 137.0}};
|
||||
auto r = fitHeading(s, 500000);
|
||||
CHECK_FALSE(r.ok);
|
||||
CHECK(r.error.find("two sightings") != std::string::npos);
|
||||
}
|
||||
|
||||
TEST_CASE("a short baseline is refused rather than fitted") {
|
||||
// Two points ALWAYS fit exactly, so R^2 would be 1 and the result would look
|
||||
// perfect. Only the baseline reveals that a count of aiming error would swing
|
||||
// counts_per_deg wildly.
|
||||
const double cpd = 983.33, at_center = 40.0;
|
||||
std::vector<HeadingSighting> s{
|
||||
{countsFor(40.0, cpd, at_center), 40.0},
|
||||
{countsFor(43.0, cpd, at_center), 43.0},
|
||||
};
|
||||
auto r = fitHeading(s, 500000);
|
||||
CHECK_FALSE(r.ok);
|
||||
CHECK(r.error.find("apart") != std::string::npos);
|
||||
CHECK(r.span_deg == doctest::Approx(3.0));
|
||||
}
|
||||
|
||||
TEST_CASE("distinct bearings at the same encoder position are refused") {
|
||||
// The gimbal did not move between sightings, so one bearing must be wrong.
|
||||
std::vector<HeadingSighting> s{{500000, 40.0}, {500000, 120.0}};
|
||||
auto r = fitHeading(s, 500000);
|
||||
CHECK_FALSE(r.ok);
|
||||
}
|
||||
|
||||
TEST_CASE("identical sightings are refused") {
|
||||
std::vector<HeadingSighting> s{{500000, 40.0}, {500000, 40.0}};
|
||||
auto r = fitHeading(s, 500000);
|
||||
CHECK_FALSE(r.ok);
|
||||
}
|
||||
|
||||
TEST_CASE("the fitted map round-trips through AxisMap") {
|
||||
const double cpd = -529.8, at_center = 137.4;
|
||||
std::vector<HeadingSighting> s{
|
||||
{countsFor(60.0, cpd, at_center), 60.0},
|
||||
{countsFor(210.0, cpd, at_center), 210.0},
|
||||
};
|
||||
auto r = fitHeading(s, 500000);
|
||||
REQUIRE(r.ok);
|
||||
|
||||
AxisMap m;
|
||||
m.counts_per_deg = r.counts_per_deg;
|
||||
m.center_count = 500000;
|
||||
m.deg_at_center = r.deg_at_center;
|
||||
m.min_deg = r.deg_at_center - 160.0;
|
||||
m.max_deg = r.deg_at_center + 160.0;
|
||||
|
||||
// The map now reads back the very bearings that were sighted.
|
||||
for (const auto& sight : s)
|
||||
CHECK(m.headingDeg(sight.counts) == doctest::Approx(sight.heading_deg).epsilon(1e-4));
|
||||
}
|
||||
|
||||
TEST_CASE("parseHeadingSightings accepts the wizard's token form") {
|
||||
std::vector<HeadingSighting> out;
|
||||
std::string err;
|
||||
|
||||
REQUIRE(parseHeadingSightings({"500123:137.4", "480000:-12"}, out, err));
|
||||
REQUIRE(out.size() == 2);
|
||||
CHECK(out[0].counts == 500123);
|
||||
CHECK(out[0].heading_deg == doctest::Approx(137.4));
|
||||
CHECK(out[1].counts == 480000);
|
||||
CHECK(out[1].heading_deg == doctest::Approx(-12.0));
|
||||
|
||||
CHECK_FALSE(parseHeadingSightings({"500123"}, out, err)); // no colon
|
||||
CHECK_FALSE(parseHeadingSightings({"500123:"}, out, err)); // no bearing
|
||||
CHECK_FALSE(parseHeadingSightings({"abc:1"}, out, err)); // counts not a number
|
||||
CHECK_FALSE(parseHeadingSightings({"1:2:3"}, out, err)); // trailing junk
|
||||
CHECK_FALSE(parseHeadingSightings({"500123x:12"}, out, err)); // trailing junk in counts
|
||||
CHECK_FALSE(err.empty());
|
||||
}
|
||||
|
|
@ -38,7 +38,8 @@ TEST_CASE("renderHelp() with no topic lists every section and entry") {
|
|||
}
|
||||
// A couple of the commands.
|
||||
CHECK(out.find("gimbal move") != std::string::npos);
|
||||
CHECK(out.find("gimbal calib") != std::string::npos);
|
||||
CHECK(out.find("gimbal pitch setup") != std::string::npos);
|
||||
CHECK(out.find("gimbal heading setup") != std::string::npos);
|
||||
}
|
||||
|
||||
TEST_CASE("renderHelp(<section>) expands that section with detail") {
|
||||
|
|
@ -52,7 +53,9 @@ TEST_CASE("renderHelp(<verb>) matches the gimbal commands, case-insensitively")
|
|||
std::string lower = join(renderHelp("gimbal"));
|
||||
std::string upper = join(renderHelp("GIMBAL"));
|
||||
CHECK(lower.find("gimbal move <yaw>,<pitch>") != std::string::npos);
|
||||
CHECK(lower.find("gimbal calib") != std::string::npos);
|
||||
CHECK(lower.find("gimbal pitch setup") != std::string::npos);
|
||||
// The removed `gimbal calib` must not linger anywhere in the catalog.
|
||||
CHECK(lower.find("gimbal calib") == std::string::npos);
|
||||
CHECK(lower == upper);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/CalibrationRoutine.h"
|
||||
#include "fgc/PitchSetup.h"
|
||||
#include "fgc/Geometry.h"
|
||||
#include "fgc/IImuSource.h"
|
||||
#include "fgc/IMotorController.h"
|
||||
|
|
@ -21,10 +21,10 @@ namespace {
|
|||
const char* kDump =
|
||||
"DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n"
|
||||
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 "
|
||||
"speed=50000 eeprom_restored=0 has_encoder=1\n"
|
||||
"speed=50000 center=500000 has_encoder=1\n"
|
||||
"DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
||||
"DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 "
|
||||
"speed=150000 eeprom_restored=0 has_encoder=1\n"
|
||||
"speed=150000 center=500000 has_encoder=1\n"
|
||||
"DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
||||
"DUMP END\n";
|
||||
|
||||
|
|
@ -81,60 +81,33 @@ struct FakeMotor : IMotorController {
|
|||
bool connected() const override { return true; }
|
||||
};
|
||||
|
||||
// Fake IMU: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw
|
||||
// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation /
|
||||
// headingReset into the shared motor event log so ordering can be asserted.
|
||||
// Fake IMU: reported pitch is the live pitch encoder / kFakeCpd, so a correct
|
||||
// routine recovers kFakeCpd exactly. Yaw is not modelled - pitch setup never
|
||||
// looks at it.
|
||||
struct FakeImu : IImuSource {
|
||||
FakeMotor& motor;
|
||||
long yaw_zero = 0;
|
||||
int scenario_type = 39; // starts on a mag-using profile
|
||||
bool has_profiles = true; // expose an available list (incl. a no-mag one)
|
||||
bool is_connected = true;
|
||||
explicit FakeImu(FakeMotor& m) : motor(m) {}
|
||||
|
||||
void start() override {}
|
||||
void stop() override {}
|
||||
bool connected() const override { return true; }
|
||||
bool connected() const override { return is_connected; }
|
||||
|
||||
std::optional<ImuSample> sample() override {
|
||||
MotorTelemetry t = motor.telemetry();
|
||||
ImuSample s;
|
||||
s.valid = true;
|
||||
s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
|
||||
double y = (t.yaw.xenc - yaw_zero) / kFakeCpd;
|
||||
s.yaw_deg = static_cast<float>(y);
|
||||
return s;
|
||||
}
|
||||
|
||||
std::optional<ImuDeviceConfig> config() const override {
|
||||
ImuDeviceConfig c;
|
||||
c.valid = true;
|
||||
c.has_scenario = true;
|
||||
c.scenario_type = static_cast<uint8_t>(scenario_type);
|
||||
if (has_profiles)
|
||||
c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
|
||||
{53, 11, "VRU_general"}};
|
||||
return c;
|
||||
}
|
||||
|
||||
void noRotation(int seconds) override {
|
||||
motor.record("noRotation:" + std::to_string(seconds));
|
||||
}
|
||||
void headingReset() override {
|
||||
motor.record("headingReset");
|
||||
std::lock_guard<std::mutex> lk(motor.mtx);
|
||||
yaw_zero = motor.tel.yaw.xenc; // current pose becomes yaw 0
|
||||
}
|
||||
bool setFilterProfile(int type) override {
|
||||
motor.record("setProfile:" + std::to_string(type));
|
||||
scenario_type = type;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
Geometry uncalibrated() {
|
||||
Geometry g;
|
||||
g.yaw = {1.0, 0, -100000.0, 100000.0};
|
||||
g.pitch = {1.0, 0, -100000.0, 100000.0};
|
||||
// center_count 0 so the fake's "counts = deg * kFakeCpd" holds exactly and
|
||||
// deg_at_center comes out as 0.
|
||||
g.yaw = {1.0, 0, 0.0, -100000.0, 100000.0};
|
||||
g.pitch = {1.0, 0, 0.0, -100000.0, 100000.0};
|
||||
return g;
|
||||
}
|
||||
|
||||
|
|
@ -146,17 +119,15 @@ CalibParams fastParams() {
|
|||
p.sample_ms = 1;
|
||||
p.settle_timeout_ms = 200;
|
||||
p.home_timeout_ms = 500;
|
||||
p.norotation_s = 0;
|
||||
p.reset_settle_ms = 2;
|
||||
return p;
|
||||
}
|
||||
|
||||
// Run to completion (or fail the test on timeout).
|
||||
void runToCompletion(CalibrationRoutine& cal) {
|
||||
void runToCompletion(PitchSetupRoutine& cal) {
|
||||
REQUIRE(cal.start());
|
||||
auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
|
||||
while (cal.running()) {
|
||||
if (std::chrono::steady_clock::now() > deadline) { FAIL("calibration did not finish"); }
|
||||
if (std::chrono::steady_clock::now() > deadline) { FAIL("pitch setup did not finish"); }
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
}
|
||||
|
|
@ -175,131 +146,120 @@ int lastIndexOf(const std::vector<std::string>& v, const std::string& needle) {
|
|||
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("calibration homes first when the gimbal is not READY") {
|
||||
TEST_CASE("pitch setup homes first when the gimbal is not READY") {
|
||||
FakeMotor motor; // starts in Boot (not ready)
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
const auto& ev = motor.events;
|
||||
REQUIRE_FALSE(ev.empty());
|
||||
CHECK(ev.front() == "HOME"); // homing is the first action
|
||||
CHECK(ev.front() == "HOME"); // homing is the first action
|
||||
CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move
|
||||
}
|
||||
|
||||
TEST_CASE("calibration does NOT home when already READY") {
|
||||
TEST_CASE("pitch setup does NOT home when already READY") {
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
CHECK(indexOf(motor.events, "HOME") == -1); // no homing issued
|
||||
}
|
||||
|
||||
TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") {
|
||||
TEST_CASE("pitch setup never moves yaw") {
|
||||
// Yaw is tied to north by `gimbal heading setup` against landmarks, not by
|
||||
// this routine. Touching it here would silently undo that calibration.
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
const auto& ev = motor.events;
|
||||
// Yaw is parked at its first position before the pitch sweep begins.
|
||||
const int first_yaw_move = indexOf(ev, "MOVE Y");
|
||||
const int first_pitch_move = indexOf(ev, "MOVE P");
|
||||
REQUIRE(first_yaw_move >= 0);
|
||||
REQUIRE(first_pitch_move >= 0);
|
||||
CHECK(first_yaw_move < first_pitch_move);
|
||||
CHECK(indexOf(motor.events, "MOVE Y") == -1);
|
||||
CHECK(indexOf(motor.events, "MOVE P") >= 0);
|
||||
|
||||
// No-mag profile switch, drift correction, then heading reset happen AFTER the
|
||||
// pitch sweep and BEFORE the yaw sweep reaches its far end.
|
||||
const int prof = indexOf(ev, "setProfile:53"); // VRU_general from the fake list
|
||||
const int norot = indexOf(ev, "noRotation");
|
||||
const int reset = indexOf(ev, "headingReset");
|
||||
REQUIRE(prof >= 0);
|
||||
REQUIRE(norot >= 0);
|
||||
REQUIRE(reset >= 0);
|
||||
CHECK(first_pitch_move < prof); // pitch calibrated before the profile switch
|
||||
CHECK(prof < norot); // switch to no-mag, then correct drift
|
||||
CHECK(norot < reset); // correct drift, then reset
|
||||
CHECK(reset < lastIndexOf(ev, "MOVE Y")); // yaw sweep continues after the reset
|
||||
|
||||
// Pitch is commanded to 0 deg before the heading reset.
|
||||
CHECK(indexOf(ev, "MOVE P 0") >= 0);
|
||||
CHECK(indexOf(ev, "MOVE P 0") < reset);
|
||||
// And the published geometry leaves the yaw map exactly as it found it.
|
||||
auto geo = cal.takeResult();
|
||||
REQUIRE(geo.has_value());
|
||||
CHECK(geo->yaw.counts_per_deg == doctest::Approx(uncalibrated().yaw.counts_per_deg));
|
||||
CHECK(geo->yaw.deg_at_center == doctest::Approx(uncalibrated().yaw.deg_at_center));
|
||||
}
|
||||
|
||||
TEST_CASE("calibration skips the profile switch when no no-mag profile exists") {
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
imu.has_profiles = false; // device reports no available profiles
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
const auto& ev = motor.events;
|
||||
CHECK(indexOf(ev, "setProfile") == -1); // nothing to switch to
|
||||
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
||||
CHECK(cal.report().valid);
|
||||
}
|
||||
|
||||
TEST_CASE("calibration recovers the fake IMU slope for both axes") {
|
||||
TEST_CASE("pitch setup recovers the fake IMU slope and reports one axis") {
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
CalibReport rep = cal.report();
|
||||
REQUIRE(rep.valid);
|
||||
CHECK(rep.all_ok);
|
||||
REQUIRE(rep.axes.size() == 2);
|
||||
for (const auto& ax : rep.axes) {
|
||||
CHECK(ax.ok);
|
||||
CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
||||
CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001));
|
||||
}
|
||||
REQUIRE(rep.axes.size() == 1);
|
||||
CHECK(rep.axes[0].axis == 'P');
|
||||
CHECK(rep.axes[0].ok);
|
||||
CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
||||
CHECK(rep.axes[0].r2 == doctest::Approx(1.0).epsilon(0.001));
|
||||
|
||||
// The new geometry is published for the session to apply.
|
||||
auto geo = cal.takeResult();
|
||||
REQUIRE(geo.has_value());
|
||||
CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
||||
CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
||||
}
|
||||
|
||||
TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") {
|
||||
TEST_CASE("pitch setup parks level when it is done") {
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
// With center_count 0 and the fake's slope, 0 deg is count 0 - and the last
|
||||
// pitch move must be to it, not to wherever the sweep happened to end.
|
||||
CHECK(motor.events.back() == "MOVE P 0");
|
||||
}
|
||||
|
||||
TEST_CASE("pitch setup aborts without an IMU rather than guessing") {
|
||||
FakeMotor motor;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
||||
motor.tel.pitch_present = true;
|
||||
FakeImu imu(motor);
|
||||
imu.is_connected = false;
|
||||
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
CHECK(motor.events.empty()); // nothing moved
|
||||
CHECK_FALSE(cal.report().valid);
|
||||
CHECK_FALSE(cal.takeResult().has_value());
|
||||
}
|
||||
|
||||
TEST_CASE("pitch setup aborts on a yaw-only gimbal") {
|
||||
FakeMotor motor;
|
||||
motor.pitch_present = false;
|
||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
||||
motor.tel.pitch_present = false;
|
||||
FakeImu imu(motor);
|
||||
|
||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
||||
runToCompletion(cal);
|
||||
|
||||
const auto& ev = motor.events;
|
||||
CHECK(indexOf(ev, "MOVE P") == -1); // no pitch moves
|
||||
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
||||
CHECK(indexOf(ev, "MOVE Y") >= 0);
|
||||
|
||||
CalibReport rep = cal.report();
|
||||
REQUIRE(rep.valid);
|
||||
REQUIRE(rep.axes.size() == 1);
|
||||
CHECK(rep.axes[0].axis == 'Y');
|
||||
CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
||||
CHECK(indexOf(motor.events, "MOVE P") == -1);
|
||||
CHECK_FALSE(cal.report().valid);
|
||||
}
|
||||
|
|
@ -51,11 +51,13 @@ struct FakeChannel : IControlChannel {
|
|||
}
|
||||
};
|
||||
|
||||
// Simple linear map: 10 counts/deg, zero at 0. Yaw clamps -90..90, pitch 0..60.
|
||||
// Simple linear map: 10 counts/deg, centre at count 0 pointing at 0 deg.
|
||||
// Reachable yaw travel -90..90, pitch 0..60.
|
||||
// Fields: counts_per_deg, center_count, deg_at_center, min_deg, max_deg.
|
||||
Geometry testGeometry() {
|
||||
Geometry g;
|
||||
g.yaw = {10.0, 0, -90.0, 90.0};
|
||||
g.pitch = {10.0, 0, 0.0, 60.0};
|
||||
g.yaw = {10.0, 0, 0.0, -90.0, 90.0};
|
||||
g.pitch = {10.0, 0, 0.0, 0.0, 60.0};
|
||||
return g;
|
||||
}
|
||||
|
||||
|
|
@ -169,7 +171,7 @@ TEST_CASE("CaptureScheduler ControlCode 1 drives yaw to the target heading") {
|
|||
CHECK(sch.controlCode() == 1);
|
||||
}
|
||||
|
||||
TEST_CASE("CaptureScheduler clamps the target heading to the soft range") {
|
||||
TEST_CASE("CaptureScheduler refuses an unreachable target heading instead of clamping") {
|
||||
long long clock = 0;
|
||||
FakeMotor motor;
|
||||
FakeCamera cam;
|
||||
|
|
@ -182,15 +184,20 @@ TEST_CASE("CaptureScheduler clamps the target heading to the soft range") {
|
|||
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
|
||||
// 200 deg is 160 deg the short way round, past the 90 deg limit: in the arc
|
||||
// this gimbal physically cannot reach.
|
||||
chan.next.target_heading = "200";
|
||||
|
||||
clock = 1600;
|
||||
sch.tick();
|
||||
CHECK(motor.cmds.back() == "MOVE 900,0");
|
||||
// No move at all. Clamping to the 90 deg limit would have imaged a direction
|
||||
// 110 deg away from the one requested and labelled it "200".
|
||||
CHECK(motor.cmds.empty());
|
||||
CHECK_FALSE(sch.moving());
|
||||
}
|
||||
|
||||
TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conversion") {
|
||||
// A `gimbal calib` updates the live geometry; the scheduler must adopt it so
|
||||
// A `gimbal heading fit` updates the live geometry; the scheduler must adopt it so
|
||||
// the scan/directed targets use the calibrated counts/deg (not the stale copy).
|
||||
long long clock = 0;
|
||||
FakeMotor motor;
|
||||
|
|
@ -220,8 +227,8 @@ TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conver
|
|||
// Recalibrate to 20 counts/deg; the same persisted heading must now resolve
|
||||
// to twice the counts — no new heading sent.
|
||||
Geometry g2;
|
||||
g2.yaw = {20.0, 0, -90.0, 90.0};
|
||||
g2.pitch = {10.0, 0, 0.0, 60.0};
|
||||
g2.yaw = {20.0, 0, 0.0, -90.0, 90.0};
|
||||
g2.pitch = {10.0, 0, 0.0, 0.0, 60.0};
|
||||
sch.setGeometry(g2);
|
||||
|
||||
clock = 2800;
|
||||
|
|
@ -309,3 +316,154 @@ TEST_CASE("CaptureScheduler abandons a waypoint after frame_timeout_ms") {
|
|||
CHECK_FALSE(sch.moving());
|
||||
CHECK(grid.index() == 1);
|
||||
}
|
||||
|
||||
// --- manual snapshot -------------------------------------------------------
|
||||
//
|
||||
// The snapshot exists so an operator aiming by hand gets exactly what a scan
|
||||
// waypoint produces. These pin the two properties that make that true: it goes
|
||||
// through the same settle gate, and it never overlaps a scan.
|
||||
|
||||
TEST_CASE("requestSnapshot captures one frame once the gimbal has settled") {
|
||||
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.setSettleWait(1000, 2000);
|
||||
// Capture inactive: this is manual mode.
|
||||
|
||||
settleAt(motor, 0, 0);
|
||||
sch.requestSnapshot();
|
||||
CHECK(sch.snapshotPending());
|
||||
|
||||
clock = 10;
|
||||
sch.tick(); // first standstill seen; starts the settle clock
|
||||
CHECK(cam.triggers == 0);
|
||||
|
||||
clock = 500;
|
||||
sch.tick(); // still inside the settle wait
|
||||
CHECK(cam.triggers == 0);
|
||||
|
||||
clock = 1100;
|
||||
sch.tick(); // settle wait elapsed
|
||||
CHECK(cam.triggers == 1);
|
||||
CHECK_FALSE(sch.snapshotPending());
|
||||
|
||||
// One request, one image: further ticks must not keep firing.
|
||||
clock = 5000;
|
||||
sch.tick();
|
||||
CHECK(cam.triggers == 1);
|
||||
}
|
||||
|
||||
TEST_CASE("a snapshot waits out a slew rather than shooting mid-move") {
|
||||
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.setSettleWait(1000, 60000); // long frame budget: the point here is the settle
|
||||
|
||||
settleAt(motor, 0, 0);
|
||||
motor.tel.yaw.standstill = false; // a jog is in flight
|
||||
sch.requestSnapshot();
|
||||
|
||||
for (clock = 10; clock < 3000; clock += 500) sch.tick();
|
||||
CHECK(cam.triggers == 0); // never fires while moving
|
||||
|
||||
motor.tel.yaw.standstill = true;
|
||||
clock = 3100;
|
||||
sch.tick(); // standstill seen, clock starts here
|
||||
CHECK(cam.triggers == 0);
|
||||
clock = 4200;
|
||||
sch.tick();
|
||||
CHECK(cam.triggers == 1);
|
||||
}
|
||||
|
||||
TEST_CASE("a scan owns the camera: snapshots are refused and never queue up") {
|
||||
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.setSettleWait(0, 0);
|
||||
settleAt(motor, 0, 0);
|
||||
|
||||
sch.requestSnapshot();
|
||||
sch.setCaptureActive(true);
|
||||
// Arming is dropped, not deferred: firing it whenever the scan happens to end
|
||||
// would attribute an image to whatever the gimbal was pointing at then.
|
||||
CHECK_FALSE(sch.snapshotPending());
|
||||
|
||||
sch.requestSnapshot(); // asked for again mid-scan
|
||||
clock = 1600;
|
||||
sch.tick(); // the scan's own move, not a snapshot
|
||||
sch.setCaptureActive(false);
|
||||
clock = 3000;
|
||||
sch.tick();
|
||||
CHECK_FALSE(sch.snapshotPending());
|
||||
}
|
||||
|
||||
TEST_CASE("the snapshot frame budget is measured from settling, not from the keypress") {
|
||||
// Pressing Space mid-slew must not spend the camera's budget on the slew.
|
||||
// Before this was fixed, a long jog could exhaust the timeout before the
|
||||
// gimbal had even stopped, and the snapshot was abandoned having never asked
|
||||
// the camera for anything.
|
||||
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.setSettleWait(1000, 2000);
|
||||
|
||||
settleAt(motor, 0, 0);
|
||||
motor.tel.yaw.standstill = false; // a long slew is in flight
|
||||
sch.requestSnapshot();
|
||||
|
||||
// Slew for far longer than the whole settle+timeout budget.
|
||||
for (clock = 10; clock < 30000; clock += 500) sch.tick();
|
||||
CHECK(sch.snapshotPending()); // still armed, not timed out
|
||||
CHECK(cam.triggers == 0);
|
||||
|
||||
motor.tel.yaw.standstill = true;
|
||||
clock = 30500;
|
||||
sch.tick(); // standstill seen; the budget starts HERE
|
||||
clock = 31600; // settle wait elapsed
|
||||
sch.tick();
|
||||
CHECK(cam.triggers == 1);
|
||||
}
|
||||
|
||||
TEST_CASE("a snapshot that times out can simply be asked for again") {
|
||||
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.setSettleWait(1000, 2000);
|
||||
settleAt(motor, 0, 0);
|
||||
|
||||
cam.ok = false; // camera has no frame to give
|
||||
sch.requestSnapshot();
|
||||
for (clock = 10; clock < 6000; clock += 250) sch.tick();
|
||||
CHECK_FALSE(sch.snapshotPending()); // gave up rather than staying armed forever
|
||||
const int attempts = cam.triggers;
|
||||
CHECK(attempts > 0); // it did keep retrying within the budget
|
||||
|
||||
// It must not keep trying after giving up.
|
||||
for (clock = 6000; clock < 12000; clock += 250) sch.tick();
|
||||
CHECK(cam.triggers == attempts);
|
||||
|
||||
// And the next press starts a fresh budget and succeeds.
|
||||
cam.ok = true;
|
||||
sch.requestSnapshot();
|
||||
clock = 12500;
|
||||
sch.tick();
|
||||
clock = 13600;
|
||||
sch.tick();
|
||||
CHECK(cam.triggers == attempts + 1);
|
||||
CHECK_FALSE(sch.snapshotPending());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,8 +29,8 @@ TestProfile fastProfile() {
|
|||
|
||||
Geometry unitGeometry() {
|
||||
Geometry g;
|
||||
g.yaw.counts_per_deg = 1000; g.yaw.zero_count = 0; g.yaw.min_deg = -90; g.yaw.max_deg = 90;
|
||||
g.pitch.counts_per_deg = 1000; g.pitch.zero_count = 0; g.pitch.min_deg = 0; g.pitch.max_deg = 60;
|
||||
g.yaw.counts_per_deg = 1000; g.yaw.center_count = 0; g.yaw.min_deg = -90; g.yaw.max_deg = 90;
|
||||
g.pitch.counts_per_deg = 1000; g.pitch.center_count = 0; g.pitch.min_deg = 0; g.pitch.max_deg = 60;
|
||||
return g;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue