88 lines
3.4 KiB
C++
88 lines
3.4 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 -> trigger 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, then asks
|
|
// the camera for an image. Whether that is a single triggered frame, and
|
|
// whether it is judged and re-shot until it passes, is decided below this
|
|
// layer (see GatedCameraSource) - the scheduler only cares that trigger()
|
|
// eventually succeeds, and retries next tick if it does not.
|
|
//
|
|
// ControlCode 0 = automatic sweep through the ScanGrid (ping-pong);
|
|
// 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_; }
|
|
|
|
void setImageRate(double rate); // images per second
|
|
double imageRate() const { return image_rate_; }
|
|
|
|
// Replace the degrees<->counts maps (e.g. after `gimbal calib`) 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;
|
|
|
|
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
|
|
bool pitch_present_ = false; // a P: segment was seen in telemetry (2-axis rig)
|
|
long yaw_target_ = 0; // current target, encoder counts
|
|
long pitch_target_ = 0;
|
|
};
|
|
|
|
} // namespace fgc
|