121 lines
5.3 KiB
C++
121 lines
5.3 KiB
C++
#pragma once
|
|
|
|
#include "fgc/Geometry.h"
|
|
#include "fgc/ICameraSource.h"
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/OperatingMode.h"
|
|
#include "fgc/ScanGrid.h"
|
|
|
|
#include <functional>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
// The capture control loop, expressed against the I/O interfaces so it can be
|
|
// unit-tested with mocks.
|
|
//
|
|
// Each tick():
|
|
// 1. reads motor telemetry (per-axis encoder counts + state),
|
|
// 2. runs the capture cycle as a move -> settle -> wait -> keep machine: when
|
|
// the interval elapses it issues an absolute `MOVE <yaw>,<pitch>` to the next
|
|
// target, waits until both axes report standstill at that target, waits a
|
|
// further settle_wait_ms so the free-running camera has exposed a frame with
|
|
// the gimbal at rest, then keeps the newest frame. If none is ready it
|
|
// retries each tick, and gives up on the waypoint after frame_timeout_ms.
|
|
//
|
|
// What it captures is the mode (OperatingMode.h): auto sweeps the ScanGrid front
|
|
// to back, repeating; pan walks panHeadings() left to right at the pan's pitch,
|
|
// then jumps back to the left edge; manual captures nothing (bar a snapshot).
|
|
// Degrees are converted to encoder counts via Geometry.
|
|
class CaptureScheduler {
|
|
public:
|
|
// now_ms: monotonic millisecond clock; defaults to steady_clock. Injectable
|
|
// for deterministic tests.
|
|
CaptureScheduler(IMotorController& motor, ICameraSource& camera, double image_rate,
|
|
Geometry geometry, ScanGrid& grid, std::function<long long()> now_ms = {});
|
|
|
|
// Switch mode. The caller (Application::setMode) has already checked that
|
|
// the gimbal can do it; this restarts the move/settle cycle from the first
|
|
// waypoint or pan position.
|
|
void setMode(Mode mode, const PanSpec& pan = {});
|
|
Mode mode() const { return mode_; }
|
|
PanSpec pan() const { return pan_; }
|
|
bool captureActive() const { return mode_ != Mode::Manual; }
|
|
|
|
// hardware.ini [Hardware] pan_step_deg: how much bearing one picture covers.
|
|
void setPanStep(double deg) { pan_step_deg_ = deg; }
|
|
double panStep() const { return pan_step_deg_; }
|
|
// The current pan's bearings, left to right, and the one being visited.
|
|
const std::vector<double>& panHeadingsNow() const { return pan_headings_; }
|
|
size_t panIndex() const { return pan_index_; }
|
|
|
|
// 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_; }
|
|
|
|
// Per-camera-profile timing (see CameraProfile): how long to wait after standstill
|
|
// before keeping a frame, and how long to keep retrying before abandoning the
|
|
// waypoint. Called on the control thread, same as tick().
|
|
void setSettleWait(int settle_ms, int timeout_ms);
|
|
int settleWaitMs() const { return settle_wait_ms_; }
|
|
|
|
// 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; }
|
|
|
|
// Run one iteration of the control logic.
|
|
void tick();
|
|
|
|
// Inspection (mainly for tests).
|
|
long yawTargetCounts() const { return yaw_target_; }
|
|
long pitchTargetCounts() const { return pitch_target_; }
|
|
bool moving() const { return moving_; }
|
|
|
|
private:
|
|
long long elapsedMs() const;
|
|
void resetTimer();
|
|
void sendMove(); // emit MOVE to (yaw_target_, pitch_target_)
|
|
bool settledAt(const MotorTelemetry& t) const;
|
|
void serviceSnapshot(const MotorTelemetry& t);
|
|
|
|
IMotorController& motor_;
|
|
ICameraSource& camera_;
|
|
Geometry geometry_;
|
|
ScanGrid& grid_;
|
|
|
|
std::function<long long()> now_ms_;
|
|
double image_rate_;
|
|
long long timer_start_ = 0;
|
|
|
|
Mode mode_ = Mode::Manual;
|
|
PanSpec pan_;
|
|
double pan_step_deg_ = 6.0;
|
|
std::vector<double> pan_headings_;
|
|
size_t pan_index_ = 0;
|
|
|
|
bool moving_ = false; // a MOVE has been issued, awaiting settle
|
|
long long settled_at_ms_ = 0; // when standstill was first seen (0 = not yet)
|
|
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 pan_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;
|
|
};
|
|
|
|
} // namespace fgc
|