207 lines
10 KiB
C++
207 lines
10 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. The interval is the time from one kept frame to
|
|
// the next: as soon as a frame is kept it issues an absolute
|
|
// `MOVE <yaw>,<pitch>` to the next target, waits until both axes report
|
|
// standstill there, waits for the exposure to settle, then waits out what
|
|
// is left of the interval and keeps the newest frame. If none is ready it
|
|
// retries each tick, and gives up on the waypoint after frame_timeout_ms.
|
|
//
|
|
// The exposure wait is settle_wait_ms; with the keep rule on (setSettleRule)
|
|
// a scan waypoint then also waits until the camera's frames say the image
|
|
// has settled (SettleRule), at most frame_timeout_ms more - after which the
|
|
// newest frame is kept anyway, with a warning.
|
|
//
|
|
// When the move and the exposure wait alone take longer than the interval,
|
|
// that waypoint warns - except the return from the last waypoint to the
|
|
// first, and the first move after a mode starts, which are long by nature.
|
|
//
|
|
// 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 - one per interval since the last, once standstill + the exposure
|
|
// wait are done - wherever the operator has aimed. The gimbal stays free to
|
|
// nudge; a frame is only kept at rest. No overrun warnings: the operator may be
|
|
// jogging. 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
|
|
// for the exposure to settle, 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_; }
|
|
// Where the current waypoint is: "moving", "settling" (exposure) or "waiting"
|
|
// (for the rest of the interval). For the status line.
|
|
const char* phase() const;
|
|
// Waypoints that warned for running past the interval since construction.
|
|
int overruns() const { return overruns_; }
|
|
|
|
private:
|
|
// One frame's way to the camera, from standstill: when the gimbal came to
|
|
// rest, when the exposure wait ended, when the trigger fell due (0 = not yet).
|
|
struct Hold {
|
|
long long settled_ms = 0;
|
|
long long ready_ms = 0;
|
|
long long due_ms = 0;
|
|
};
|
|
enum class Shot { Waiting, Captured, TimedOut };
|
|
|
|
long long elapsedMs() const;
|
|
void resetTimer();
|
|
void sendMove(); // emit MOVE to (yaw_target_, pitch_target_)
|
|
bool settledAt(const MotorTelemetry& t) const;
|
|
bool startNextMove(); // pick the next target and MOVE; false if none
|
|
void finishWaypoint();
|
|
std::string waypointLabel() const; // "waypoint 3/15" / "pan position 2/6"
|
|
void serviceSnapshot(const MotorTelemetry& t);
|
|
void serviceContinuous(const MotorTelemetry& t);
|
|
|
|
// Standstill seen: begin the exposure wait.
|
|
void beginHold(Hold& h);
|
|
// Whether the exposure wait that began at h.settled_ms is over (sets
|
|
// h.ready_ms): settle_wait_ms, then with `keep_rule` the keep rule too.
|
|
bool dwellOver(Hold& h, bool keep_rule);
|
|
// The keep rule, fed the frames since it last looked: true once it has
|
|
// decided - settled, at the exposure limits, timed out (with a warning), or
|
|
// no frame data to judge.
|
|
bool ruleDecided(long long since_settled);
|
|
// The save gate, with one log line when a hold starts and one when it ends.
|
|
bool roomToSave();
|
|
// Trigger until a frame comes or frame_timeout_ms passes since it fell due.
|
|
Shot fire(Hold& h);
|
|
|
|
// The part a snapshot and a continuous-capture frame share: wait for the
|
|
// gimbal to stand still, then the exposure wait, then (continuous only) the
|
|
// rest of the interval, then trigger until the frame budget runs out.
|
|
Shot shootAtRest(const MotorTelemetry& t, Hold& h, bool wait_interval);
|
|
|
|
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_;
|
|
double interval_ms_; // 1000 / image_rate_; 0 = never
|
|
long long timer_start_ = 0; // the last frame kept or given up, or the mode start
|
|
|
|
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, its frame not yet kept
|
|
Hold hold_; // the current waypoint's frame
|
|
long long move_sent_ms_ = 0;
|
|
bool first_move_ = true; // no move yet since setMode()
|
|
bool quiet_overrun_ = false; // this waypoint is a long move by nature
|
|
int overruns_ = 0;
|
|
bool save_held_ = false; // the next move waited for saving to catch up
|
|
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
|
|
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
|
|
Hold snapshot_hold_;
|
|
bool continuous_ = false; // `capture on`
|
|
Hold continuous_hold_;
|
|
long yaw_target_ = 0; // current target, encoder counts
|
|
long pitch_target_ = 0;
|
|
};
|
|
|
|
} // namespace fgc
|