266 lines
9.2 KiB
C++
266 lines
9.2 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/PitchSetup.h"
|
|
#include "fgc/Geometry.h"
|
|
#include "fgc/IImuSource.h"
|
|
#include "fgc/IMotorController.h"
|
|
|
|
#include <chrono>
|
|
#include <mutex>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
using namespace fgc;
|
|
|
|
namespace {
|
|
|
|
// A two-axis firmware DUMP with soft limits (subset of the real format; enough
|
|
// for parseDump to expose lim_neg/lim_pos per axis).
|
|
const char* kDump =
|
|
"DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n"
|
|
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 "
|
|
"speed=50000 center=500000 has_encoder=1\n"
|
|
"DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
|
"DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 "
|
|
"speed=150000 center=500000 has_encoder=1\n"
|
|
"DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
|
"DUMP END\n";
|
|
|
|
// Counts-per-degree the fake IMU reports, so a clean sweep recovers this slope.
|
|
constexpr double kFakeCpd = 10000.0;
|
|
|
|
// Fake motor: records every command in order, and "executes" HOME/MOVE instantly
|
|
// (axis jumps to target, standstill + READY) so waitSettle/waitHomed return at
|
|
// once. IMU angle is derived from the live encoder position (see FakeImu).
|
|
struct FakeImu;
|
|
struct FakeMotor : IMotorController {
|
|
mutable std::mutex mtx;
|
|
MotorTelemetry tel;
|
|
std::vector<std::string> events; // unified, ordered log (motor + imu)
|
|
bool pitch_present = true;
|
|
bool have_dump = true;
|
|
|
|
void start() override {}
|
|
void stop() override {}
|
|
|
|
void record(const std::string& s) {
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
events.push_back(s);
|
|
}
|
|
|
|
void sendCommand(const std::string& c) override {
|
|
record(c);
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
std::istringstream iss(c);
|
|
std::string verb;
|
|
iss >> verb;
|
|
if (verb == "HOME") {
|
|
tel.yaw.state = AxisState::Ready; tel.yaw.standstill = true;
|
|
tel.pitch.state = AxisState::Ready; tel.pitch.standstill = true;
|
|
tel.pitch_present = pitch_present;
|
|
} else if (verb == "MOVE") {
|
|
std::string axis;
|
|
long target = 0;
|
|
iss >> axis >> target;
|
|
AxisTelemetry& a = (axis == "Y") ? tel.yaw : tel.pitch;
|
|
a.xenc = target;
|
|
a.standstill = true;
|
|
a.state = AxisState::Ready;
|
|
}
|
|
}
|
|
|
|
MotorTelemetry telemetry() override {
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
return tel;
|
|
}
|
|
std::string lastDump() override { return have_dump ? kDump : ""; }
|
|
std::string lastDiag() override { return ""; }
|
|
unsigned diagSeq() const override { return 0; }
|
|
bool connected() const override { return true; }
|
|
};
|
|
|
|
// Fake IMU: reported pitch is the live pitch encoder / kFakeCpd, so a correct
|
|
// routine recovers kFakeCpd exactly. Yaw is not modelled - pitch setup never
|
|
// looks at it.
|
|
struct FakeImu : IImuSource {
|
|
FakeMotor& motor;
|
|
bool is_connected = true;
|
|
explicit FakeImu(FakeMotor& m) : motor(m) {}
|
|
|
|
void start() override {}
|
|
void stop() override {}
|
|
bool connected() const override { return is_connected; }
|
|
|
|
std::optional<ImuSample> sample() override {
|
|
MotorTelemetry t = motor.telemetry();
|
|
ImuSample s;
|
|
s.valid = true;
|
|
s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
|
|
return s;
|
|
}
|
|
};
|
|
|
|
Geometry uncalibrated() {
|
|
Geometry g;
|
|
// center_count 0 so the fake's "counts = deg * kFakeCpd" holds exactly and
|
|
// deg_at_center comes out as 0.
|
|
g.yaw = {1.0, 0, 0.0, -100000.0, 100000.0};
|
|
g.pitch = {1.0, 0, 0.0, -100000.0, 100000.0};
|
|
return g;
|
|
}
|
|
|
|
// Fast timings so a full run completes in well under a second.
|
|
CalibParams fastParams() {
|
|
CalibParams p;
|
|
p.positions = 4;
|
|
p.dwell_ms = 4;
|
|
p.sample_ms = 1;
|
|
p.settle_timeout_ms = 200;
|
|
p.home_timeout_ms = 500;
|
|
return p;
|
|
}
|
|
|
|
// Run to completion (or fail the test on timeout).
|
|
void runToCompletion(PitchSetupRoutine& cal) {
|
|
REQUIRE(cal.start());
|
|
auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
|
|
while (cal.running()) {
|
|
if (std::chrono::steady_clock::now() > deadline) { FAIL("pitch setup did not finish"); }
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
}
|
|
}
|
|
|
|
int indexOf(const std::vector<std::string>& v, const std::string& needle) {
|
|
for (int i = 0; i < static_cast<int>(v.size()); ++i)
|
|
if (v[i].find(needle) != std::string::npos) return i;
|
|
return -1;
|
|
}
|
|
int lastIndexOf(const std::vector<std::string>& v, const std::string& needle) {
|
|
int found = -1;
|
|
for (int i = 0; i < static_cast<int>(v.size()); ++i)
|
|
if (v[i].find(needle) != std::string::npos) found = i;
|
|
return found;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST_CASE("pitch setup homes first when the gimbal is not READY") {
|
|
FakeMotor motor; // starts in Boot (not ready)
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
const auto& ev = motor.events;
|
|
REQUIRE_FALSE(ev.empty());
|
|
CHECK(ev.front() == "HOME"); // homing is the first action
|
|
CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move
|
|
}
|
|
|
|
TEST_CASE("pitch setup does NOT home when already READY") {
|
|
FakeMotor motor;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CHECK(indexOf(motor.events, "HOME") == -1); // no homing issued
|
|
}
|
|
|
|
TEST_CASE("pitch setup never moves yaw") {
|
|
// Yaw is tied to north by `gimbal heading setup` against landmarks, not by
|
|
// this routine. Touching it here would silently undo that calibration.
|
|
FakeMotor motor;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CHECK(indexOf(motor.events, "MOVE Y") == -1);
|
|
CHECK(indexOf(motor.events, "MOVE P") >= 0);
|
|
|
|
// And the published geometry leaves the yaw map exactly as it found it.
|
|
auto geo = cal.takeResult();
|
|
REQUIRE(geo.has_value());
|
|
CHECK(geo->yaw.counts_per_deg == doctest::Approx(uncalibrated().yaw.counts_per_deg));
|
|
CHECK(geo->yaw.deg_at_center == doctest::Approx(uncalibrated().yaw.deg_at_center));
|
|
}
|
|
|
|
TEST_CASE("pitch setup recovers the fake IMU slope and reports one axis") {
|
|
FakeMotor motor;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CalibReport rep = cal.report();
|
|
REQUIRE(rep.valid);
|
|
CHECK(rep.all_ok);
|
|
REQUIRE(rep.axes.size() == 1);
|
|
CHECK(rep.axes[0].axis == 'P');
|
|
CHECK(rep.axes[0].ok);
|
|
CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
CHECK(rep.axes[0].r2 == doctest::Approx(1.0).epsilon(0.001));
|
|
|
|
auto geo = cal.takeResult();
|
|
REQUIRE(geo.has_value());
|
|
CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
}
|
|
|
|
TEST_CASE("pitch setup parks level when it is done") {
|
|
FakeMotor motor;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
// With center_count 0 and the fake's slope, 0 deg is count 0 - and the last
|
|
// pitch move must be to it, not to wherever the sweep happened to end.
|
|
CHECK(motor.events.back() == "MOVE P 0");
|
|
}
|
|
|
|
TEST_CASE("pitch setup aborts without an IMU rather than guessing") {
|
|
FakeMotor motor;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
imu.is_connected = false;
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CHECK(motor.events.empty()); // nothing moved
|
|
CHECK_FALSE(cal.report().valid);
|
|
CHECK_FALSE(cal.takeResult().has_value());
|
|
}
|
|
|
|
TEST_CASE("pitch setup aborts on a yaw-only gimbal") {
|
|
FakeMotor motor;
|
|
motor.pitch_present = false;
|
|
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
motor.tel.pitch_present = false;
|
|
FakeImu imu(motor);
|
|
|
|
PitchSetupRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CHECK(indexOf(motor.events, "MOVE P") == -1);
|
|
CHECK_FALSE(cal.report().valid);
|
|
}
|