111 lines
4.7 KiB
C++
111 lines
4.7 KiB
C++
#pragma once
|
|
|
|
#include "fgc/Geometry.h"
|
|
#include "fgc/ICameraSource.h"
|
|
#include "fgc/IControlChannel.h"
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/ScanGrid.h"
|
|
|
|
#include <functional>
|
|
#include <string>
|
|
|
|
namespace fgc {
|
|
|
|
// The capture control loop, expressed against the I/O interfaces so it can be
|
|
// unit-tested with mocks.
|
|
//
|
|
// Each tick():
|
|
// 1. polls the control channel (updates control code / target heading,
|
|
// echoes the code back as status),
|
|
// 2. reads motor telemetry (per-axis encoder counts + state),
|
|
// 3. 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.
|
|
//
|
|
// ControlCode 0 = automatic sweep through the ScanGrid (front to back, repeating);
|
|
// ControlCode 1 = drive yaw to the MQTT-supplied target heading (pitch held).
|
|
// Degrees are converted to encoder counts via Geometry.
|
|
class CaptureScheduler {
|
|
public:
|
|
// now_ms: monotonic millisecond clock; defaults to steady_clock. Injectable
|
|
// for deterministic tests.
|
|
CaptureScheduler(IMotorController& motor, ICameraSource& camera, IControlChannel& channel,
|
|
double image_rate, Geometry geometry, ScanGrid& grid,
|
|
std::function<long long()> now_ms = {});
|
|
|
|
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_; }
|
|
|
|
// 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).
|
|
int controlCode() const { return control_code_; }
|
|
std::string targetHeading() const { return target_heading_; }
|
|
long yawTargetCounts() const { return yaw_target_; }
|
|
long pitchTargetCounts() const { return pitch_target_; }
|
|
bool moving() const { return moving_; }
|
|
|
|
private:
|
|
long long elapsedMs() const;
|
|
void resetTimer();
|
|
void sendMove(); // emit MOVE to (yaw_target_, pitch_target_)
|
|
bool settledAt(const MotorTelemetry& t) const;
|
|
void serviceSnapshot(const MotorTelemetry& t);
|
|
|
|
IMotorController& motor_;
|
|
ICameraSource& camera_;
|
|
IControlChannel& channel_;
|
|
Geometry geometry_;
|
|
ScanGrid& grid_;
|
|
|
|
std::function<long long()> now_ms_;
|
|
double image_rate_;
|
|
long long timer_start_ = 0;
|
|
|
|
bool capture_active_ = false;
|
|
int control_code_ = 0;
|
|
std::string target_heading_ = "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 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;
|
|
};
|
|
|
|
} // namespace fgc
|