119 lines
4.0 KiB
C++
119 lines
4.0 KiB
C++
#include "fgc/CaptureScheduler.h"
|
|
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
|
|
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,
|
|
IControlChannel& channel, double image_rate, Geometry geometry,
|
|
ScanGrid& grid, std::function<long long()> now_ms)
|
|
: motor_(motor),
|
|
camera_(camera),
|
|
channel_(channel),
|
|
geometry_(geometry),
|
|
grid_(grid),
|
|
now_ms_(now_ms ? std::move(now_ms) : steadyNowMs),
|
|
image_rate_(image_rate) {
|
|
timer_start_ = now_ms_();
|
|
}
|
|
|
|
void CaptureScheduler::setCaptureActive(bool active) {
|
|
capture_active_ = active;
|
|
moving_ = false; // restart the move/settle cycle cleanly
|
|
resetTimer();
|
|
}
|
|
void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }
|
|
|
|
long long CaptureScheduler::elapsedMs() const { return now_ms_() - timer_start_; }
|
|
void CaptureScheduler::resetTimer() { timer_start_ = now_ms_(); }
|
|
|
|
void CaptureScheduler::sendMove() {
|
|
motor_.sendCommand("MOVE " + std::to_string(yaw_target_) + "," + std::to_string(pitch_target_));
|
|
}
|
|
|
|
bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
|
|
return t.yaw.standstill && t.pitch.standstill &&
|
|
std::labs(t.yaw.xenc - yaw_target_) <= kSettleTolCounts &&
|
|
std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
|
|
}
|
|
|
|
void CaptureScheduler::tick() {
|
|
// 1. Remote control input.
|
|
ControlCommand cmd = channel_.poll();
|
|
if (cmd.control_code_available) {
|
|
control_code_ = cmd.control_code;
|
|
LOG_TRACE_CAT(LogCat::Control) << "control_code -> " << control_code_;
|
|
channel_.publishStatus(control_code_);
|
|
}
|
|
if (cmd.heading_available) {
|
|
target_heading_ = cmd.target_heading;
|
|
LOG_TRACE_CAT(LogCat::Control) << "target_heading -> " << target_heading_;
|
|
}
|
|
|
|
// 2. Telemetry.
|
|
MotorTelemetry t = motor_.telemetry();
|
|
|
|
// 3. Capture cycle. Only ControlCode 0 (sweep) and 1 (directed) act.
|
|
if (control_code_ != 0 && control_code_ != 1) return;
|
|
if (!capture_active_) return;
|
|
|
|
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 (control_code_ == 0) {
|
|
if (grid_.empty()) return;
|
|
const ScanPoint& p = grid_.current();
|
|
yaw_target_ = geometry_.yaw.toCounts(p.yaw_deg);
|
|
pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
|
|
LOG_TRACE_CAT(LogCat::Control)
|
|
<< "sweep -> grid yaw=" << p.yaw_deg << " pitch=" << p.pitch_deg;
|
|
} else { // ControlCode 1: directed yaw, pitch held at current position.
|
|
try {
|
|
yaw_target_ = geometry_.yaw.toCounts(std::stod(target_heading_));
|
|
} catch (const std::exception&) {
|
|
return; // bad heading; wait for a valid one
|
|
}
|
|
pitch_target_ = t.pitch.xenc;
|
|
LOG_TRACE_CAT(LogCat::Control) << "directed -> heading " << target_heading_;
|
|
}
|
|
sendMove();
|
|
moving_ = true;
|
|
resetTimer();
|
|
return;
|
|
}
|
|
|
|
// Moving: trigger once both axes have settled at the target.
|
|
if (settledAt(t)) {
|
|
LOG_TRACE_CAT(LogCat::Control)
|
|
<< "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc << "; triggering";
|
|
if (camera_.trigger()) {
|
|
moving_ = false;
|
|
if (control_code_ == 0) grid_.advance();
|
|
resetTimer();
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|