411 lines
16 KiB
C++
411 lines
16 KiB
C++
#include "fgc/CaptureScheduler.h"
|
|
|
|
#include "fgc/CommandParser.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <chrono>
|
|
#include <cstdint>
|
|
#include <cstdlib>
|
|
#include <iomanip>
|
|
#include <optional>
|
|
#include <sstream>
|
|
|
|
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;
|
|
|
|
// Seconds with one decimal, for the overrun warning.
|
|
std::string seconds(double ms) {
|
|
std::ostringstream o;
|
|
o << std::fixed << std::setprecision(1) << ms / 1000.0 << " s";
|
|
return o.str();
|
|
}
|
|
|
|
} // 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),
|
|
interval_ms_(image_rate > 0.0 ? 1000.0 / image_rate : 0.0) {
|
|
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
|
|
hold_ = {};
|
|
// The first move goes from wherever the gimbal is, so it may be long: no
|
|
// overrun warning for it.
|
|
first_move_ = true;
|
|
rule_started_ = false;
|
|
// 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;
|
|
// Continuous capture is a manual-mode activity: any mode change ends it, so
|
|
// a later return to manual never resumes shooting nobody asked for.
|
|
continuous_ = false;
|
|
continuous_hold_ = {};
|
|
// The first frame comes no sooner than one interval after the mode starts.
|
|
resetTimer();
|
|
}
|
|
|
|
void CaptureScheduler::setContinuous(bool on) {
|
|
continuous_ = on;
|
|
continuous_hold_ = {};
|
|
// The first frame comes one interval after switching on, as the first scan
|
|
// waypoint does after `mode auto`.
|
|
resetTimer();
|
|
}
|
|
|
|
void CaptureScheduler::setSettleRule(bool on, const SettleRule::Params& params) {
|
|
use_rule_ = on;
|
|
rule_.setParams(params);
|
|
rule_started_ = false;
|
|
}
|
|
|
|
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.
|
|
motor_.sendCommand(moveCommand(yaw_target_, pitch_present_ ? std::optional<long>(pitch_target_)
|
|
: std::nullopt));
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
bool CaptureScheduler::roomToSave() {
|
|
const bool room = !can_save_ || can_save_();
|
|
if (!room && !holding_) {
|
|
holding_ = true;
|
|
LOG_WARN << "saving is behind: holding the scan here until it catches up";
|
|
} else if (room && holding_) {
|
|
holding_ = false;
|
|
LOG_INFO << "saving caught up: the scan carries on";
|
|
}
|
|
return room;
|
|
}
|
|
|
|
const char* CaptureScheduler::phase() const {
|
|
if (!moving_ || hold_.settled_ms == 0) return "moving";
|
|
return hold_.ready_ms == 0 ? "settling" : "waiting";
|
|
}
|
|
|
|
std::string CaptureScheduler::waypointLabel() const {
|
|
if (mode_ == Mode::Pan)
|
|
return "pan position " + std::to_string(pan_index_ + 1) + "/" +
|
|
std::to_string(pan_headings_.size());
|
|
return "waypoint " + std::to_string(grid_.index() + 1) + "/" + std::to_string(grid_.size());
|
|
}
|
|
|
|
void CaptureScheduler::requestSnapshot() {
|
|
snapshot_pending_ = true;
|
|
snapshot_hold_ = {};
|
|
}
|
|
|
|
void CaptureScheduler::beginHold(Hold& h) {
|
|
h = {};
|
|
h.settled_ms = now_ms_();
|
|
}
|
|
|
|
// From here on, watch the frames the camera delivers until brightness says the
|
|
// image has settled, or the frame budget runs out.
|
|
bool CaptureScheduler::ruleDecided(long long since_settled) {
|
|
if (!rule_started_) {
|
|
rule_started_ = true;
|
|
rule_.reset();
|
|
rule_seq_ = camera_.frameSeq();
|
|
// No frame data from this camera: keep the fixed wait's behaviour.
|
|
if (rule_seq_ == 0) rule_seq_ = UINT64_MAX;
|
|
}
|
|
if (rule_seq_ == UINT64_MAX) return true;
|
|
SettleRule::Verdict v = SettleRule::Verdict::Waiting;
|
|
for (const auto& f : camera_.framesSince(rule_seq_)) {
|
|
rule_seq_ = f.seq;
|
|
v = rule_.add(f);
|
|
if (v != SettleRule::Verdict::Waiting) break;
|
|
}
|
|
if (v == SettleRule::Verdict::Waiting) {
|
|
if (since_settled < settle_wait_ms_ + frame_timeout_ms_) return false; // keep watching
|
|
LOG_WARN << waypointLabel() << ": the image did not settle within " << frame_timeout_ms_
|
|
<< " ms of standstill; keeping the newest frame";
|
|
} else {
|
|
LOG_TRACE_CAT(LogCat::Control)
|
|
<< "image settled after " << since_settled << " ms"
|
|
<< (v == SettleRule::Verdict::AtLimit ? " (exposure and gain at their limit)" : "");
|
|
}
|
|
rule_seq_ = UINT64_MAX; // decided
|
|
return true;
|
|
}
|
|
|
|
bool CaptureScheduler::dwellOver(Hold& h, bool keep_rule) {
|
|
if (h.ready_ms != 0) return true;
|
|
const long long since = now_ms_() - h.settled_ms;
|
|
if (since < settle_wait_ms_) return false;
|
|
if (keep_rule && use_rule_ && !ruleDecided(since)) return false;
|
|
h.ready_ms = now_ms_();
|
|
return true;
|
|
}
|
|
|
|
CaptureScheduler::Shot CaptureScheduler::fire(Hold& h) {
|
|
if (h.due_ms == 0) h.due_ms = now_ms_();
|
|
if (camera_.trigger()) return Shot::Captured;
|
|
if (now_ms_() - h.due_ms >= frame_timeout_ms_) return Shot::TimedOut;
|
|
return Shot::Waiting; // no fresh frame yet; retry next tick
|
|
}
|
|
|
|
// Wait for rest and the exposure, then shoot - the tail of a scan waypoint
|
|
// without the move. A snapshot and a continuous-capture frame both come through
|
|
// here, so either is exactly what a scan waypoint produces: same file, same
|
|
// CamEvent.
|
|
//
|
|
// Standstill only, no target check: the operator aims, so there is no target.
|
|
// Movement restarts the wait. The frame budget is measured from when the trigger
|
|
// fell due, not from the request - a request made mid-slew must not spend the
|
|
// camera's budget on the slew.
|
|
CaptureScheduler::Shot CaptureScheduler::shootAtRest(const MotorTelemetry& t, Hold& h,
|
|
bool wait_interval) {
|
|
const bool standstill = t.yaw.standstill && (!pitch_present_ || t.pitch.standstill);
|
|
if (!standstill) {
|
|
h = {}; // still slewing (e.g. a jog is in flight)
|
|
return Shot::Waiting;
|
|
}
|
|
if (h.settled_ms == 0) {
|
|
beginHold(h);
|
|
return Shot::Waiting;
|
|
}
|
|
if (!dwellOver(h, false)) return Shot::Waiting;
|
|
if (wait_interval) {
|
|
if (interval_ms_ <= 0.0 || elapsedMs() < interval_ms_) return Shot::Waiting;
|
|
// Continuous capture holds, as the scan does, while saving is behind.
|
|
if (h.due_ms == 0 && !roomToSave()) return Shot::Waiting;
|
|
}
|
|
return fire(h);
|
|
}
|
|
|
|
void CaptureScheduler::serviceSnapshot(const MotorTelemetry& t) {
|
|
switch (shootAtRest(t, snapshot_hold_, false)) {
|
|
case Shot::Waiting:
|
|
return;
|
|
case Shot::Captured:
|
|
LOG_INFO << "snapshot captured";
|
|
break;
|
|
case Shot::TimedOut:
|
|
LOG_WARN << "snapshot: no frame within " << frame_timeout_ms_
|
|
<< " ms; send 'snapshot' (Space) again";
|
|
break;
|
|
}
|
|
snapshot_pending_ = false;
|
|
snapshot_hold_ = {};
|
|
}
|
|
|
|
// The scan's cycle without the move: one frame per interval since the last,
|
|
// once at rest and the exposure has settled.
|
|
void CaptureScheduler::serviceContinuous(const MotorTelemetry& t) {
|
|
switch (shootAtRest(t, continuous_hold_, true)) {
|
|
case Shot::Waiting:
|
|
return;
|
|
case Shot::Captured:
|
|
LOG_INFO << "continuous capture: captured";
|
|
break;
|
|
case Shot::TimedOut:
|
|
LOG_WARN << "continuous capture: no frame within " << frame_timeout_ms_
|
|
<< " ms; trying again next interval";
|
|
break;
|
|
}
|
|
continuous_hold_ = {};
|
|
resetTimer();
|
|
}
|
|
|
|
// Pick the next target, skipping unreachable ones in this same tick (a skip
|
|
// costs no time and does not restart the interval), and MOVE there.
|
|
bool CaptureScheduler::startNextMove() {
|
|
// A move that starts a pass again - back from the last waypoint to the first -
|
|
// is long by nature and the interval is not expected to hold for it; nor for
|
|
// the first move after a mode starts, from wherever the gimbal was.
|
|
bool pass_start = false;
|
|
std::optional<long> yc;
|
|
if (mode_ == Mode::Auto) {
|
|
if (grid_.empty()) return false;
|
|
pass_start = grid_.index() == 0 && grid_.size() > 1;
|
|
// 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. Try each at most once per tick, so a grid with none reachable
|
|
// just idles.
|
|
for (size_t tries = 0; tries < grid_.size(); ++tries) {
|
|
yc = geometry_.yaw.headingToCounts(grid_.current().yaw_deg);
|
|
if (yc) break;
|
|
if (!unreachable_warned_) {
|
|
unreachable_warned_ = true;
|
|
LOG_WARN << "scan waypoint " << (grid_.index() + 1) << "/" << grid_.size()
|
|
<< " heading " << grid_.current().yaw_deg
|
|
<< " deg is outside the reachable yaw arc; skipping it (further "
|
|
"unreachable waypoints are skipped silently)";
|
|
}
|
|
grid_.advance();
|
|
}
|
|
if (!yc) return false;
|
|
const ScanPoint& p = grid_.current();
|
|
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 false;
|
|
pass_start = pan_index_ == 0 && pan_headings_.size() > 1;
|
|
// Skip positions outside the reachable arc, as the sweep does.
|
|
for (size_t tries = 0; tries < pan_headings_.size(); ++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 false;
|
|
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)";
|
|
}
|
|
// Time spent waiting for saving is already announced, so not an overrun.
|
|
quiet_overrun_ = first_move_ || pass_start || save_held_;
|
|
first_move_ = false;
|
|
save_held_ = false;
|
|
sendMove();
|
|
moving_ = true;
|
|
hold_ = {};
|
|
rule_started_ = false;
|
|
move_sent_ms_ = now_ms_();
|
|
return true;
|
|
}
|
|
|
|
// The waypoint's frame is kept or given up: on to the next, and the interval
|
|
// counts from now.
|
|
void CaptureScheduler::finishWaypoint() {
|
|
moving_ = false;
|
|
hold_ = {};
|
|
rule_started_ = false;
|
|
if (mode_ == Mode::Auto) grid_.advance();
|
|
else if (!pan_headings_.empty()) pan_index_ = (pan_index_ + 1) % pan_headings_.size();
|
|
resetTimer();
|
|
}
|
|
|
|
void CaptureScheduler::tick() {
|
|
// 1. Telemetry.
|
|
MotorTelemetry t = motor_.telemetry();
|
|
pitch_present_ = t.pitch_present;
|
|
|
|
// 2. Manual: a snapshot, else continuous capture. Only in manual, so a
|
|
// request can never inject a frame into a running sweep or fight it for
|
|
// the settle window. A snapshot goes first; continuous capture resumes
|
|
// on its own clock afterwards.
|
|
if (mode_ == Mode::Manual) {
|
|
if (snapshot_pending_) serviceSnapshot(t);
|
|
else if (continuous_) serviceContinuous(t);
|
|
return;
|
|
}
|
|
|
|
// 3. Capture cycle: the next scan waypoint (auto) or pan position (pan).
|
|
// Move straight away - while there is room to save what it will capture -
|
|
// and wait out what is left of the interval at the end.
|
|
if (interval_ms_ <= 0.0) return;
|
|
if (!moving_) {
|
|
if (!roomToSave()) {
|
|
save_held_ = true;
|
|
return;
|
|
}
|
|
startNextMove();
|
|
return;
|
|
}
|
|
|
|
// Moving: wait for standstill at the target.
|
|
if (hold_.settled_ms == 0) {
|
|
if (!settledAt(t)) return;
|
|
beginHold(hold_);
|
|
LOG_TRACE_CAT(LogCat::Control)
|
|
<< "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc
|
|
<< "; waiting " << settle_wait_ms_ << " ms"
|
|
<< (use_rule_ ? " and for the image to settle" : "") << " before capture";
|
|
return;
|
|
}
|
|
|
|
// The camera free-runs, so this wait 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.
|
|
if (hold_.ready_ms == 0) {
|
|
if (!dwellOver(hold_, true)) return;
|
|
const long long used = hold_.ready_ms - timer_start_;
|
|
if (static_cast<double>(used) > interval_ms_ && !quiet_overrun_) {
|
|
++overruns_;
|
|
LOG_WARN << waypointLabel() << ": " << seconds(static_cast<double>(used))
|
|
<< " since the last picture, over the " << seconds(interval_ms_)
|
|
<< " interval (move "
|
|
<< seconds(static_cast<double>(hold_.settled_ms - move_sent_ms_))
|
|
<< ", exposure "
|
|
<< seconds(static_cast<double>(hold_.ready_ms - hold_.settled_ms)) << ")";
|
|
}
|
|
}
|
|
|
|
// Then whatever is left of the interval.
|
|
if (static_cast<double>(elapsedMs()) < interval_ms_) return;
|
|
|
|
switch (fire(hold_)) {
|
|
case Shot::Waiting:
|
|
return;
|
|
case Shot::Captured:
|
|
LOG_INFO << "captured " << waypointLabel();
|
|
break;
|
|
case Shot::TimedOut:
|
|
// 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; skipping "
|
|
<< waypointLabel();
|
|
break;
|
|
}
|
|
finishWaypoint();
|
|
}
|
|
|
|
} // namespace fgc
|