237 lines
9.9 KiB
C++
237 lines
9.9 KiB
C++
#include "fgc/CaptureScheduler.h"
|
|
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <chrono>
|
|
#include <cstdlib>
|
|
#include <optional>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
long long steadyNowMs() {
|
|
using namespace std::chrono;
|
|
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|
}
|
|
|
|
// How close (encoder counts) an axis must be to its target, in addition to
|
|
// reporting standstill, to count as "settled". Generous vs. the firmware hold
|
|
// deadband (~200 counts).
|
|
constexpr long kSettleTolCounts = 600;
|
|
|
|
} // namespace
|
|
|
|
CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
|
|
double image_rate, Geometry geometry, ScanGrid& grid,
|
|
std::function<long long()> now_ms)
|
|
: motor_(motor),
|
|
camera_(camera),
|
|
geometry_(geometry),
|
|
grid_(grid),
|
|
now_ms_(now_ms ? std::move(now_ms) : steadyNowMs),
|
|
image_rate_(image_rate) {
|
|
timer_start_ = now_ms_();
|
|
}
|
|
|
|
void CaptureScheduler::setMode(Mode mode, const PanSpec& pan) {
|
|
mode_ = mode;
|
|
pan_ = pan;
|
|
pan_headings_ = mode == Mode::Pan ? panHeadings(pan, pan_step_deg_) : std::vector<double>{};
|
|
pan_index_ = 0;
|
|
pan_unreachable_warned_ = false;
|
|
moving_ = false; // restart the move/settle cycle cleanly
|
|
settled_at_ms_ = 0;
|
|
// Drop any armed snapshot: a scan starting means the operator moved on, and
|
|
// keeping it would fire an image at some unrelated later moment when the scan
|
|
// stops - attributed to whatever the gimbal happened to be pointing at then.
|
|
snapshot_pending_ = false;
|
|
resetTimer();
|
|
}
|
|
void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }
|
|
|
|
void CaptureScheduler::setSettleWait(int settle_ms, int timeout_ms) {
|
|
settle_wait_ms_ = settle_ms < 0 ? 0 : settle_ms;
|
|
frame_timeout_ms_ = timeout_ms < 0 ? 0 : timeout_ms;
|
|
}
|
|
|
|
long long CaptureScheduler::elapsedMs() const { return now_ms_() - timer_start_; }
|
|
void CaptureScheduler::resetTimer() { timer_start_ = now_ms_(); }
|
|
|
|
void CaptureScheduler::sendMove() {
|
|
// On a 1-axis (yaw-only) rig the firmware rejects the two-axis `MOVE y,p`
|
|
// form ("ERR axis not present") and moves nothing, so address yaw alone.
|
|
if (pitch_present_) {
|
|
motor_.sendCommand("MOVE " + std::to_string(yaw_target_) + "," +
|
|
std::to_string(pitch_target_));
|
|
} else {
|
|
motor_.sendCommand("MOVE Y " + std::to_string(yaw_target_));
|
|
}
|
|
}
|
|
|
|
bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
|
|
bool yaw_ok = t.yaw.standstill && std::labs(t.yaw.xenc - yaw_target_) <= kSettleTolCounts;
|
|
// A 1-axis rig never reports pitch standstill, so don't gate the capture on it.
|
|
if (!t.pitch_present) return yaw_ok;
|
|
return yaw_ok && t.pitch.standstill &&
|
|
std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
|
|
}
|
|
|
|
void CaptureScheduler::requestSnapshot() {
|
|
snapshot_pending_ = true;
|
|
snapshot_settled_ms_ = 0;
|
|
}
|
|
|
|
// One frame, on request, through exactly the path a scan waypoint takes: the
|
|
// same standstill + settle wait, the same camera trigger, and therefore the same
|
|
// .jxl on disk and the same CamEvent on MQTT. Reusing the path rather than
|
|
// triggering the camera directly is the point - a snapshot that differed from a
|
|
// scan frame would be useless as a reference for aiming.
|
|
void CaptureScheduler::serviceSnapshot(const MotorTelemetry& t) {
|
|
const bool standstill = t.yaw.standstill && (!pitch_present_ || t.pitch.standstill);
|
|
if (!standstill) {
|
|
snapshot_settled_ms_ = 0; // still slewing (e.g. a jog is in flight)
|
|
return;
|
|
}
|
|
if (snapshot_settled_ms_ == 0) {
|
|
snapshot_settled_ms_ = now_ms_();
|
|
return;
|
|
}
|
|
const long long since_settled = now_ms_() - snapshot_settled_ms_;
|
|
if (since_settled < settle_wait_ms_) return;
|
|
|
|
if (camera_.trigger()) {
|
|
LOG_INFO << "snapshot captured";
|
|
snapshot_pending_ = false;
|
|
return;
|
|
}
|
|
// No fresh frame yet. Give up on the same budget a waypoint gets - measured
|
|
// from settling, as the message says, not from the keypress: the operator may
|
|
// have pressed Space mid-slew, and counting the slew against the camera would
|
|
// abandon a snapshot the camera never had a chance at.
|
|
if (since_settled > settle_wait_ms_ + frame_timeout_ms_) {
|
|
LOG_WARN << "snapshot: no frame within " << frame_timeout_ms_
|
|
<< " ms of settling; press Space again";
|
|
snapshot_pending_ = false;
|
|
}
|
|
}
|
|
|
|
void CaptureScheduler::tick() {
|
|
// 1. Telemetry.
|
|
MotorTelemetry t = motor_.telemetry();
|
|
pitch_present_ = t.pitch_present;
|
|
|
|
// 2. Manual snapshot. Only in manual, so a keypress can never inject a frame
|
|
// into a running sweep or fight it for the settle window.
|
|
if (mode_ == Mode::Manual) {
|
|
if (snapshot_pending_) serviceSnapshot(t);
|
|
return;
|
|
}
|
|
|
|
// 3. Capture cycle: the next scan waypoint (auto) or pan position (pan).
|
|
const double interval_ms = image_rate_ > 0.0 ? 1000.0 / image_rate_ : 0.0;
|
|
|
|
if (!moving_) {
|
|
// Issue the next move once the interval has elapsed.
|
|
if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;
|
|
|
|
if (mode_ == Mode::Auto) {
|
|
if (grid_.empty()) return;
|
|
const ScanPoint& p = grid_.current();
|
|
// Yaw travel is under a full turn, so some bearings are unreachable.
|
|
// Skip such a waypoint rather than clamping to the nearest limit,
|
|
// which would image a direction nobody asked for and label it with
|
|
// the one they did.
|
|
auto yc = geometry_.yaw.headingToCounts(p.yaw_deg);
|
|
if (!yc) {
|
|
if (!unreachable_warned_) {
|
|
unreachable_warned_ = true;
|
|
LOG_WARN << "scan waypoint " << (grid_.index() + 1) << "/" << grid_.size()
|
|
<< " heading " << p.yaw_deg
|
|
<< " deg is outside the reachable yaw arc; skipping it (further "
|
|
"unreachable waypoints are skipped silently)";
|
|
}
|
|
grid_.advance();
|
|
resetTimer();
|
|
return;
|
|
}
|
|
yaw_target_ = *yc;
|
|
pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
|
|
// One INFO line per move so the operator can follow the scan in the
|
|
// log without enabling the control wire-trace.
|
|
LOG_INFO << "scan move " << (grid_.index() + 1) << "/" << grid_.size()
|
|
<< ": yaw " << p.yaw_deg << " pitch " << p.pitch_deg << " (deg)";
|
|
} else { // pan: the next position, left to right, at the pan's pitch
|
|
if (pan_headings_.empty()) return;
|
|
// Skip positions outside the reachable arc, as the sweep does; try
|
|
// each at most once per tick so a pan with none reachable just idles.
|
|
std::optional<long> yc;
|
|
for (size_t tries = 0; tries < pan_headings_.size() && !yc; ++tries) {
|
|
yc = geometry_.yaw.headingToCounts(pan_headings_[pan_index_]);
|
|
if (yc) break;
|
|
if (!pan_unreachable_warned_) {
|
|
pan_unreachable_warned_ = true;
|
|
LOG_WARN << "pan position " << (pan_index_ + 1) << "/" << pan_headings_.size()
|
|
<< " heading " << pan_headings_[pan_index_]
|
|
<< " deg is outside the reachable yaw arc; skipping it (further "
|
|
"unreachable positions are skipped silently)";
|
|
}
|
|
pan_index_ = (pan_index_ + 1) % pan_headings_.size();
|
|
}
|
|
if (!yc) return;
|
|
yaw_target_ = *yc;
|
|
pitch_target_ = geometry_.pitch.toCounts(pan_.pitch);
|
|
LOG_INFO << "pan move " << (pan_index_ + 1) << "/" << pan_headings_.size()
|
|
<< ": yaw " << pan_headings_[pan_index_] << " pitch " << pan_.pitch
|
|
<< " (deg)";
|
|
}
|
|
sendMove();
|
|
moving_ = true;
|
|
settled_at_ms_ = 0;
|
|
resetTimer();
|
|
return;
|
|
}
|
|
|
|
// Moving: wait for standstill at the target, then hold still a moment before
|
|
// keeping a frame.
|
|
if (settled_at_ms_ == 0) {
|
|
if (!settledAt(t)) return;
|
|
settled_at_ms_ = now_ms_();
|
|
LOG_TRACE_CAT(LogCat::Control)
|
|
<< "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc << "; waiting "
|
|
<< settle_wait_ms_ << " ms before capture";
|
|
return;
|
|
}
|
|
|
|
// The camera free-runs, so this pause is what makes the kept frame one that was
|
|
// exposed with the gimbal at rest (no slew blur) and with the on-camera auto
|
|
// exposure settled on this waypoint's light.
|
|
const long long since_settled = now_ms_() - settled_at_ms_;
|
|
if (since_settled < settle_wait_ms_) return;
|
|
|
|
if (camera_.trigger()) {
|
|
if (mode_ == Mode::Auto)
|
|
LOG_INFO << "captured waypoint " << (grid_.index() + 1) << "/" << grid_.size();
|
|
else
|
|
LOG_INFO << "captured pan position " << (pan_index_ + 1) << "/"
|
|
<< pan_headings_.size();
|
|
} else if (since_settled < settle_wait_ms_ + frame_timeout_ms_) {
|
|
return; // no fresh frame yet; retry next tick
|
|
} else {
|
|
// Never park on one waypoint: a stalled camera would otherwise stop the
|
|
// whole sweep rather than costing it a single image.
|
|
LOG_WARN << "no frame within " << frame_timeout_ms_ << " ms of settling; skipping "
|
|
<< (mode_ == Mode::Auto ? "waypoint " : "pan position ")
|
|
<< (mode_ == Mode::Auto ? grid_.index() + 1 : pan_index_ + 1) << "/"
|
|
<< (mode_ == Mode::Auto ? grid_.size() : pan_headings_.size());
|
|
}
|
|
|
|
moving_ = false;
|
|
settled_at_ms_ = 0;
|
|
if (mode_ == Mode::Auto) grid_.advance();
|
|
else if (!pan_headings_.empty()) pan_index_ = (pan_index_ + 1) % pan_headings_.size();
|
|
resetTimer();
|
|
}
|
|
|
|
} // namespace fgc
|