470 lines
16 KiB
C++
470 lines
16 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/CaptureScheduler.h"
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
using namespace fgc;
|
|
|
|
namespace {
|
|
|
|
struct FakeMotor : IMotorController {
|
|
MotorTelemetry tel;
|
|
std::vector<std::string> cmds;
|
|
FakeMotor() { tel.pitch_present = true; } // 2-axis rig by default; 1-axis tests clear this
|
|
|
|
void start() override {}
|
|
void stop() override {}
|
|
void sendCommand(const std::string& c) override { cmds.push_back(c); }
|
|
MotorTelemetry telemetry() override { return tel; }
|
|
std::string lastDump() override { return ""; }
|
|
std::string lastDiag() override { return ""; }
|
|
unsigned diagSeq() const override { return 0; }
|
|
bool connected() const override { return true; }
|
|
};
|
|
|
|
struct FakeCamera : ICameraSource {
|
|
int triggers = 0;
|
|
bool ok = true;
|
|
void open() override {}
|
|
void close() override {}
|
|
void start() override {}
|
|
void stop() override {}
|
|
bool trigger() override { ++triggers; return ok; }
|
|
void setFrameCallback(FrameCallback) override {}
|
|
int cameraCount() const override { return 1; }
|
|
};
|
|
|
|
struct FakeChannel : IControlChannel {
|
|
ControlCommand next;
|
|
int last_status = -1;
|
|
bool connect() override { return true; }
|
|
void disconnect() override {}
|
|
bool connected() const override { return true; }
|
|
void publishStatus(int c) override { last_status = c; }
|
|
void publishCamEvent(const CamEvent&) override {}
|
|
ControlCommand poll() override {
|
|
ControlCommand c = next;
|
|
next = {};
|
|
return c;
|
|
}
|
|
};
|
|
|
|
// Simple linear map: 10 counts/deg, centre at count 0 pointing at 0 deg.
|
|
// Reachable yaw travel -90..90, pitch 0..60.
|
|
// Fields: counts_per_deg, center_count, deg_at_center, min_deg, max_deg.
|
|
Geometry testGeometry() {
|
|
Geometry g;
|
|
g.yaw = {10.0, 0, 0.0, -90.0, 90.0};
|
|
g.pitch = {10.0, 0, 0.0, 0.0, 60.0};
|
|
return g;
|
|
}
|
|
|
|
// Mark both axes settled at the scheduler's current target.
|
|
void settleAt(FakeMotor& m, long yaw, long pitch) {
|
|
m.tel.yaw.xenc = yaw; m.tel.yaw.standstill = true; m.tel.yaw.state = AxisState::Ready;
|
|
m.tel.pitch.xenc = pitch; m.tel.pitch.standstill = true; m.tel.pitch.state = AxisState::Ready;
|
|
m.tel.pitch_present = true;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST_CASE("CaptureScheduler sweeps the scan grid with MOVE + settle + trigger") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}}); // -> yaw counts 0, then 900
|
|
|
|
CaptureScheduler sch(motor, cam, chan, 1.0 /*img/s -> 1000ms*/, testGeometry(), grid,
|
|
[&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setCaptureActive(true);
|
|
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 0;
|
|
|
|
// Before the interval elapses: nothing moves, but status is echoed.
|
|
clock = 500;
|
|
sch.tick();
|
|
CHECK(motor.cmds.empty());
|
|
CHECK(chan.last_status == 0);
|
|
|
|
// After the interval: MOVE to the first grid point (0,0) is issued.
|
|
clock = 1600;
|
|
sch.tick();
|
|
REQUIRE(motor.cmds.size() == 1);
|
|
CHECK(motor.cmds[0] == "MOVE 0,0");
|
|
CHECK(sch.moving());
|
|
CHECK(cam.triggers == 0); // not yet settled
|
|
|
|
// Report standstill at the target -> camera fires, cursor advances.
|
|
settleAt(motor, 0, 0);
|
|
clock = 1700;
|
|
sch.tick(); // first tick records standstill
|
|
clock = 1710;
|
|
sch.tick(); // next one keeps the frame
|
|
CHECK(cam.triggers == 1);
|
|
CHECK_FALSE(sch.moving());
|
|
|
|
// Next interval: MOVE to the second grid point (90 deg -> 900 counts).
|
|
clock = 2800;
|
|
sch.tick();
|
|
CHECK(motor.cmds.back() == "MOVE 900,0");
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler scans a 1-axis (yaw-only) rig with yaw-only MOVE/settle") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
motor.tel.pitch_present = false; // yaw-only gimbal: no P: segment in telemetry
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setCaptureActive(true);
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 0;
|
|
|
|
// The MOVE must address yaw alone, not the two-axis form a 1-axis firmware rejects.
|
|
clock = 1600;
|
|
sch.tick();
|
|
REQUIRE(motor.cmds.size() == 1);
|
|
CHECK(motor.cmds[0] == "MOVE Y 0");
|
|
|
|
// Settle on yaw only (pitch never reports standstill on this rig) -> capture fires.
|
|
motor.tel.yaw.xenc = 0;
|
|
motor.tel.yaw.standstill = true;
|
|
motor.tel.yaw.state = AxisState::Ready;
|
|
clock = 1700;
|
|
sch.tick();
|
|
clock = 1710;
|
|
sch.tick();
|
|
CHECK(cam.triggers == 1);
|
|
|
|
clock = 2800;
|
|
sch.tick();
|
|
CHECK(motor.cmds.back() == "MOVE Y 900");
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler ControlCode 1 drives yaw to the target heading") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setCaptureActive(true);
|
|
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 1;
|
|
chan.next.heading_available = true;
|
|
chan.next.target_heading = "45"; // 45 deg * 10 = 450 counts
|
|
|
|
clock = 1600;
|
|
sch.tick();
|
|
REQUIRE(!motor.cmds.empty());
|
|
CHECK(motor.cmds.back() == "MOVE 450,0");
|
|
CHECK(sch.controlCode() == 1);
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler refuses an unreachable target heading instead of clamping") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setCaptureActive(true);
|
|
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 1;
|
|
chan.next.heading_available = true;
|
|
// 200 deg is 160 deg the short way round, past the 90 deg limit: in the arc
|
|
// this gimbal physically cannot reach.
|
|
chan.next.target_heading = "200";
|
|
|
|
clock = 1600;
|
|
sch.tick();
|
|
// No move at all. Clamping to the 90 deg limit would have imaged a direction
|
|
// 110 deg away from the one requested and labelled it "200".
|
|
CHECK(motor.cmds.empty());
|
|
CHECK_FALSE(sch.moving());
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conversion") {
|
|
// A `gimbal heading fit` updates the live geometry; the scheduler must adopt it so
|
|
// the scan/directed targets use the calibrated counts/deg (not the stale copy).
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setCaptureActive(true);
|
|
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 1;
|
|
chan.next.heading_available = true;
|
|
chan.next.target_heading = "45"; // 45 deg
|
|
|
|
clock = 1600;
|
|
sch.tick();
|
|
CHECK(motor.cmds.back() == "MOVE 450,0"); // 45 * 10 counts/deg
|
|
|
|
// Settle + trigger so the scheduler is ready to move again.
|
|
settleAt(motor, 450, 0);
|
|
clock = 1700;
|
|
sch.tick(); // records standstill
|
|
clock = 1710;
|
|
sch.tick(); // keeps the frame
|
|
|
|
// Recalibrate to 20 counts/deg; the same persisted heading must now resolve
|
|
// to twice the counts — no new heading sent.
|
|
Geometry g2;
|
|
g2.yaw = {20.0, 0, 0.0, -90.0, 90.0};
|
|
g2.pitch = {10.0, 0, 0.0, 0.0, 60.0};
|
|
sch.setGeometry(g2);
|
|
|
|
clock = 2800;
|
|
sch.tick();
|
|
CHECK(motor.cmds.back() == "MOVE 900,0"); // 45 * 20 counts/deg
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler stays idle when capture inactive") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
// capture not active
|
|
|
|
clock = 5000;
|
|
sch.tick();
|
|
CHECK(motor.cmds.empty());
|
|
CHECK(cam.triggers == 0);
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler holds still for settle_wait_ms before keeping a frame") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
sch.setCaptureActive(true);
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 0;
|
|
|
|
clock = 1600;
|
|
sch.tick();
|
|
REQUIRE(motor.cmds.size() == 1);
|
|
|
|
// Standstill starts the wait; the camera is not touched during it.
|
|
settleAt(motor, 0, 0);
|
|
clock = 2000;
|
|
sch.tick();
|
|
CHECK(cam.triggers == 0);
|
|
clock = 2900; // 900 ms after settling: still inside the wait
|
|
sch.tick();
|
|
CHECK(cam.triggers == 0);
|
|
|
|
// Past the wait, the newest frame is kept and the sweep advances.
|
|
clock = 3100;
|
|
sch.tick();
|
|
CHECK(cam.triggers == 1);
|
|
CHECK_FALSE(sch.moving());
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler abandons a waypoint after frame_timeout_ms") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
cam.ok = false; // camera never has a fresh frame
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
sch.setCaptureActive(true);
|
|
chan.next.control_code_available = true;
|
|
chan.next.control_code = 0;
|
|
|
|
clock = 1600;
|
|
sch.tick();
|
|
settleAt(motor, 0, 0);
|
|
clock = 1700;
|
|
sch.tick();
|
|
|
|
// Retries through the timeout window without giving up.
|
|
clock = 3500; // 1800 ms after settling: past the wait, inside the timeout
|
|
sch.tick();
|
|
CHECK(sch.moving());
|
|
CHECK(grid.index() == 0);
|
|
|
|
// Past wait + timeout, the waypoint is skipped rather than blocking the sweep.
|
|
clock = 4800;
|
|
sch.tick();
|
|
CHECK_FALSE(sch.moving());
|
|
CHECK(grid.index() == 1);
|
|
}
|
|
|
|
// --- manual snapshot -------------------------------------------------------
|
|
//
|
|
// The snapshot exists so an operator aiming by hand gets exactly what a scan
|
|
// waypoint produces. These pin the two properties that make that true: it goes
|
|
// through the same settle gate, and it never overlaps a scan.
|
|
|
|
TEST_CASE("requestSnapshot captures one frame once the gimbal has settled") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
// Capture inactive: this is manual mode.
|
|
|
|
settleAt(motor, 0, 0);
|
|
sch.requestSnapshot();
|
|
CHECK(sch.snapshotPending());
|
|
|
|
clock = 10;
|
|
sch.tick(); // first standstill seen; starts the settle clock
|
|
CHECK(cam.triggers == 0);
|
|
|
|
clock = 500;
|
|
sch.tick(); // still inside the settle wait
|
|
CHECK(cam.triggers == 0);
|
|
|
|
clock = 1100;
|
|
sch.tick(); // settle wait elapsed
|
|
CHECK(cam.triggers == 1);
|
|
CHECK_FALSE(sch.snapshotPending());
|
|
|
|
// One request, one image: further ticks must not keep firing.
|
|
clock = 5000;
|
|
sch.tick();
|
|
CHECK(cam.triggers == 1);
|
|
}
|
|
|
|
TEST_CASE("a snapshot waits out a slew rather than shooting mid-move") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 60000); // long frame budget: the point here is the settle
|
|
|
|
settleAt(motor, 0, 0);
|
|
motor.tel.yaw.standstill = false; // a jog is in flight
|
|
sch.requestSnapshot();
|
|
|
|
for (clock = 10; clock < 3000; clock += 500) sch.tick();
|
|
CHECK(cam.triggers == 0); // never fires while moving
|
|
|
|
motor.tel.yaw.standstill = true;
|
|
clock = 3100;
|
|
sch.tick(); // standstill seen, clock starts here
|
|
CHECK(cam.triggers == 0);
|
|
clock = 4200;
|
|
sch.tick();
|
|
CHECK(cam.triggers == 1);
|
|
}
|
|
|
|
TEST_CASE("a scan owns the camera: snapshots are refused and never queue up") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
settleAt(motor, 0, 0);
|
|
|
|
sch.requestSnapshot();
|
|
sch.setCaptureActive(true);
|
|
// Arming is dropped, not deferred: firing it whenever the scan happens to end
|
|
// would attribute an image to whatever the gimbal was pointing at then.
|
|
CHECK_FALSE(sch.snapshotPending());
|
|
|
|
sch.requestSnapshot(); // asked for again mid-scan
|
|
clock = 1600;
|
|
sch.tick(); // the scan's own move, not a snapshot
|
|
sch.setCaptureActive(false);
|
|
clock = 3000;
|
|
sch.tick();
|
|
CHECK_FALSE(sch.snapshotPending());
|
|
}
|
|
|
|
TEST_CASE("the snapshot frame budget is measured from settling, not from the keypress") {
|
|
// Pressing Space mid-slew must not spend the camera's budget on the slew.
|
|
// Before this was fixed, a long jog could exhaust the timeout before the
|
|
// gimbal had even stopped, and the snapshot was abandoned having never asked
|
|
// the camera for anything.
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
|
|
settleAt(motor, 0, 0);
|
|
motor.tel.yaw.standstill = false; // a long slew is in flight
|
|
sch.requestSnapshot();
|
|
|
|
// Slew for far longer than the whole settle+timeout budget.
|
|
for (clock = 10; clock < 30000; clock += 500) sch.tick();
|
|
CHECK(sch.snapshotPending()); // still armed, not timed out
|
|
CHECK(cam.triggers == 0);
|
|
|
|
motor.tel.yaw.standstill = true;
|
|
clock = 30500;
|
|
sch.tick(); // standstill seen; the budget starts HERE
|
|
clock = 31600; // settle wait elapsed
|
|
sch.tick();
|
|
CHECK(cam.triggers == 1);
|
|
}
|
|
|
|
TEST_CASE("a snapshot that times out can simply be asked for again") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
FakeChannel chan;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
settleAt(motor, 0, 0);
|
|
|
|
cam.ok = false; // camera has no frame to give
|
|
sch.requestSnapshot();
|
|
for (clock = 10; clock < 6000; clock += 250) sch.tick();
|
|
CHECK_FALSE(sch.snapshotPending()); // gave up rather than staying armed forever
|
|
const int attempts = cam.triggers;
|
|
CHECK(attempts > 0); // it did keep retrying within the budget
|
|
|
|
// It must not keep trying after giving up.
|
|
for (clock = 6000; clock < 12000; clock += 250) sch.tick();
|
|
CHECK(cam.triggers == attempts);
|
|
|
|
// And the next press starts a fresh budget and succeeds.
|
|
cam.ok = true;
|
|
sch.requestSnapshot();
|
|
clock = 12500;
|
|
sch.tick();
|
|
clock = 13600;
|
|
sch.tick();
|
|
CHECK(cam.triggers == attempts + 1);
|
|
CHECK_FALSE(sch.snapshotPending());
|
|
}
|