453 lines
16 KiB
C++
453 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; }
|
|
};
|
|
|
|
// 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;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}}); // -> yaw counts 0, then 900
|
|
|
|
CaptureScheduler sch(motor, cam, 1.0 /*img/s -> 1000ms*/, testGeometry(), grid,
|
|
[&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setMode(Mode::Auto);
|
|
|
|
|
|
// Before the interval elapses: nothing moves.
|
|
clock = 500;
|
|
sch.tick();
|
|
CHECK(motor.cmds.empty());
|
|
|
|
// 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;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0); // these predate the settle wait; timing has its own test
|
|
sch.setMode(Mode::Auto);
|
|
|
|
// 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");
|
|
}
|
|
|
|
// Walk one capture: the interval elapses and a MOVE goes out, then the gimbal
|
|
// settles on it and the frame is kept. Returns the MOVE.
|
|
std::string captureOnce(CaptureScheduler& sch, FakeMotor& motor, long long& clock) {
|
|
clock += 1100;
|
|
sch.tick();
|
|
REQUIRE(!motor.cmds.empty());
|
|
const std::string move = motor.cmds.back();
|
|
settleAt(motor, sch.yawTargetCounts(), sch.pitchTargetCounts());
|
|
clock += 10;
|
|
sch.tick(); // records standstill
|
|
clock += 10;
|
|
sch.tick(); // keeps the frame
|
|
return move;
|
|
}
|
|
|
|
TEST_CASE("pan walks its positions left to right, then jumps back to the left edge") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
sch.setPanStep(6.0);
|
|
// 0 +/- 15 at 6 deg: 345, 351, 357, 3, 9, 15 - six pictures, across north.
|
|
sch.setMode(Mode::Pan, PanSpec{0.0, 30.0, 20.0});
|
|
REQUIRE(sch.panHeadingsNow().size() == 6);
|
|
|
|
std::vector<std::string> moves;
|
|
for (int i = 0; i < 7; ++i) moves.push_back(captureOnce(sch, motor, clock));
|
|
// 10 counts/deg; pitch 20 deg -> 200 counts, whatever pitch the gimbal had.
|
|
CHECK(moves[0] == "MOVE -150,200");
|
|
CHECK(moves[1] == "MOVE -90,200");
|
|
CHECK(moves[2] == "MOVE -30,200");
|
|
CHECK(moves[3] == "MOVE 30,200");
|
|
CHECK(moves[4] == "MOVE 90,200");
|
|
CHECK(moves[5] == "MOVE 150,200");
|
|
CHECK(moves[6] == "MOVE -150,200"); // back to the left edge
|
|
CHECK(cam.triggers == 7);
|
|
}
|
|
|
|
TEST_CASE("a pan narrower than one picture stays on its heading") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
sch.setMode(Mode::Pan, PanSpec{45.0, 5.0, 0.0});
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 450,0");
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 450,0");
|
|
CHECK(cam.triggers == 2);
|
|
}
|
|
|
|
TEST_CASE("a pan skips positions outside the reachable arc instead of clamping them") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
// 84, 90, 96: the last is past the 90 deg limit. Clamping it to 90 would
|
|
// image 90 twice and label one of them 96.
|
|
sch.setMode(Mode::Pan, PanSpec{90.0, 12.0, 0.0});
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 840,0");
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 900,0");
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 840,0");
|
|
}
|
|
|
|
TEST_CASE("a pan with nothing reachable moves nothing") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
// 200 deg is 160 deg the short way round, past the 90 deg limit.
|
|
sch.setMode(Mode::Pan, PanSpec{200.0, 0.0, 0.0});
|
|
clock = 1600;
|
|
sch.tick();
|
|
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 pan targets use the calibrated counts/deg (not the stale copy).
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
sch.setMode(Mode::Pan, PanSpec{45.0, 0.0, 0.0});
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 450,0"); // 45 * 10 counts/deg
|
|
|
|
// Recalibrate to 20 counts/deg; the same heading must now resolve to twice
|
|
// the counts.
|
|
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);
|
|
CHECK(captureOnce(sch, motor, clock) == "MOVE 900,0"); // 45 * 20 counts/deg
|
|
}
|
|
|
|
TEST_CASE("CaptureScheduler stays idle in manual mode") {
|
|
long long clock = 0;
|
|
FakeMotor motor;
|
|
FakeCamera cam;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
// manual: the mode it starts in
|
|
|
|
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;
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
sch.setMode(Mode::Auto);
|
|
|
|
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
|
|
ScanGrid grid({{0.0, 0.0}, {90.0, 0.0}});
|
|
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(1000, 2000);
|
|
sch.setMode(Mode::Auto);
|
|
|
|
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;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 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;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 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;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 1.0, testGeometry(), grid, [&] { return clock; });
|
|
sch.setSettleWait(0, 0);
|
|
settleAt(motor, 0, 0);
|
|
|
|
sch.requestSnapshot();
|
|
sch.setMode(Mode::Auto);
|
|
// 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.setMode(Mode::Manual);
|
|
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;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 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;
|
|
ScanGrid grid({{0.0, 0.0}});
|
|
CaptureScheduler sch(motor, cam, 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());
|
|
}
|