163 lines
7.8 KiB
C++
163 lines
7.8 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 "fgc/SettleRule.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 or
|
|
// continuous capture - both of which shoot wherever the operator has aimed.
|
|
// 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_; }
|
|
|
|
// Continuous capture (`capture on|off`): in manual, keep a frame on the scan's
|
|
// own cycle - the interval, then standstill + settle wait - wherever the
|
|
// operator has aimed. The gimbal stays free to nudge; a frame is only kept at
|
|
// rest. Ignored outside manual, and cleared by any setMode().
|
|
void setContinuous(bool on);
|
|
bool continuous() const { return continuous_; }
|
|
|
|
double imageRate() const { return image_rate_; } // images per second
|
|
|
|
// Is there room to save another frame (ImagePipeline::canAccept)? Asked
|
|
// before moving to the next waypoint and before keeping a continuous-capture
|
|
// frame. While it says no, the scan holds where it is, so a scan that
|
|
// outruns saving slows down instead of losing pictures or filling memory.
|
|
// A requested snapshot is never held. Unset: always room.
|
|
void setSaveGate(std::function<bool()> can_save) { can_save_ = std::move(can_save); }
|
|
// True while the scan is held for saving to catch up.
|
|
bool holdingForSave() const { return holding_; }
|
|
|
|
// 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_; }
|
|
|
|
// The keep rule for scan waypoints (profile setting settle_rule). Off: keep
|
|
// the newest frame once settle_wait_ms has passed, as always. On: after
|
|
// settle_wait_ms (now the mechanical minimum), wait until the camera's
|
|
// frames say the image has settled (SettleRule), within frame_timeout_ms; a
|
|
// camera that reports no frame data falls back to the fixed wait.
|
|
void setSettleRule(bool on, const SettleRule::Params& params = {});
|
|
bool settleRuleOn() const { return use_rule_; }
|
|
|
|
// 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);
|
|
void serviceContinuous(const MotorTelemetry& t);
|
|
|
|
// The part a snapshot and a continuous-capture frame share: wait for the
|
|
// gimbal to stand still, then the settle wait, then trigger until the frame
|
|
// budget runs out. `settled_ms` is that request's own settle clock.
|
|
enum class Shot { Waiting, Captured, TimedOut };
|
|
Shot shootAtRest(const MotorTelemetry& t, long long& settled_ms);
|
|
// The save gate, with one log line when a hold starts and one when it ends.
|
|
bool roomToSave();
|
|
|
|
IMotorController& motor_;
|
|
ICameraSource& camera_;
|
|
Geometry geometry_;
|
|
ScanGrid& grid_;
|
|
|
|
std::function<long long()> now_ms_;
|
|
std::function<bool()> can_save_;
|
|
bool holding_ = false;
|
|
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 use_rule_ = false; // keep by SettleRule, not after a fixed wait
|
|
SettleRule rule_;
|
|
bool rule_started_ = false; // this waypoint's rule has its first frame number
|
|
uint64_t rule_seq_ = 0; // last frame fed to the rule
|
|
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)
|
|
bool continuous_ = false; // `capture on`
|
|
long long continuous_settled_ms_ = 0;
|
|
long yaw_target_ = 0; // current target, encoder counts
|
|
long pitch_target_ = 0;
|
|
};
|
|
|
|
} // namespace fgc
|