81 lines
2.6 KiB
C++
81 lines
2.6 KiB
C++
#include "fgc/CaptureScheduler.h"
|
|
|
|
#include <chrono>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
long long steadyNowMs() {
|
|
using namespace std::chrono;
|
|
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|
}
|
|
|
|
// Whether a software trigger should fire given the current telemetry.
|
|
//
|
|
// NOTE: this preserves the original behaviour of triggering while the gimbal
|
|
// reports is_moving == 1. That looks counter-intuitive (one might expect to
|
|
// trigger once stopped). See docs/known-issues.md #7 - kept as-is pending
|
|
// confirmation of the firmware's is_moving semantics.
|
|
bool shouldTrigger(const MotorTelemetry& t) { return t.is_moving == 1; }
|
|
|
|
} // namespace
|
|
|
|
CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
|
|
IControlChannel& channel, double image_rate,
|
|
std::function<long long()> now_ms)
|
|
: motor_(motor),
|
|
camera_(camera),
|
|
channel_(channel),
|
|
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; }
|
|
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::tick() {
|
|
// 1. Remote control input.
|
|
ControlCommand cmd = channel_.poll();
|
|
if (cmd.control_code_available) {
|
|
control_code_ = cmd.control_code;
|
|
channel_.publishStatus(control_code_);
|
|
}
|
|
if (cmd.heading_available) {
|
|
target_heading_ = cmd.target_heading;
|
|
}
|
|
|
|
// 2. Telemetry.
|
|
MotorTelemetry t = motor_.telemetry();
|
|
|
|
// 3. Capture cycle. The interval gate is 1000/rate milliseconds.
|
|
if (control_code_ != 0 && control_code_ != 1) return;
|
|
|
|
const double interval_ms = image_rate_ > 0.0 ? 1000.0 / image_rate_ : 0.0;
|
|
|
|
if (capture_active_ && interval_ms > 0.0 && elapsedMs() > interval_ms) {
|
|
if (t.is_moving == 1) {
|
|
resetTimer();
|
|
trigger_after_stopping_ = true;
|
|
if (control_code_ == 0)
|
|
motor_.sendCommand("p");
|
|
else // control_code_ == 1
|
|
motor_.sendCommand("kd" + target_heading_);
|
|
}
|
|
}
|
|
|
|
if (trigger_after_stopping_ && elapsedMs() > 100) {
|
|
if (shouldTrigger(t)) {
|
|
if (camera_.trigger()) {
|
|
trigger_after_stopping_ = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|