306 lines
11 KiB
C++
306 lines
11 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/CalibrationRoutine.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 eeprom_restored=0 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 eeprom_restored=0 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: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw
|
|
// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation /
|
|
// headingReset into the shared motor event log so ordering can be asserted.
|
|
struct FakeImu : IImuSource {
|
|
FakeMotor& motor;
|
|
long yaw_zero = 0;
|
|
int scenario_type = 39; // starts on a mag-using profile
|
|
bool has_profiles = true; // expose an available list (incl. a no-mag one)
|
|
explicit FakeImu(FakeMotor& m) : motor(m) {}
|
|
|
|
void start() override {}
|
|
void stop() override {}
|
|
bool connected() const override { return true; }
|
|
|
|
std::optional<ImuSample> sample() override {
|
|
MotorTelemetry t = motor.telemetry();
|
|
ImuSample s;
|
|
s.valid = true;
|
|
s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
|
|
double y = (t.yaw.xenc - yaw_zero) / kFakeCpd;
|
|
s.yaw_deg = static_cast<float>(y);
|
|
return s;
|
|
}
|
|
|
|
std::optional<ImuDeviceConfig> config() const override {
|
|
ImuDeviceConfig c;
|
|
c.valid = true;
|
|
c.has_scenario = true;
|
|
c.scenario_type = static_cast<uint8_t>(scenario_type);
|
|
if (has_profiles)
|
|
c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
|
|
{53, 11, "VRU_general"}};
|
|
return c;
|
|
}
|
|
|
|
void noRotation(int seconds) override {
|
|
motor.record("noRotation:" + std::to_string(seconds));
|
|
}
|
|
void headingReset() override {
|
|
motor.record("headingReset");
|
|
std::lock_guard<std::mutex> lk(motor.mtx);
|
|
yaw_zero = motor.tel.yaw.xenc; // current pose becomes yaw 0
|
|
}
|
|
bool setFilterProfile(int type) override {
|
|
motor.record("setProfile:" + std::to_string(type));
|
|
scenario_type = type;
|
|
return true;
|
|
}
|
|
};
|
|
|
|
Geometry uncalibrated() {
|
|
Geometry g;
|
|
g.yaw = {1.0, 0, -100000.0, 100000.0};
|
|
g.pitch = {1.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;
|
|
p.norotation_s = 0;
|
|
p.reset_settle_ms = 2;
|
|
return p;
|
|
}
|
|
|
|
// Run to completion (or fail the test on timeout).
|
|
void runToCompletion(CalibrationRoutine& 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("calibration 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("calibration homes first when the gimbal is not READY") {
|
|
FakeMotor motor; // starts in Boot (not ready)
|
|
motor.tel.pitch_present = true;
|
|
FakeImu imu(motor);
|
|
|
|
CalibrationRoutine 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("calibration 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);
|
|
|
|
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CHECK(indexOf(motor.events, "HOME") == -1); // no homing issued
|
|
}
|
|
|
|
TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") {
|
|
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);
|
|
|
|
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
const auto& ev = motor.events;
|
|
// Yaw is parked at its first position before the pitch sweep begins.
|
|
const int first_yaw_move = indexOf(ev, "MOVE Y");
|
|
const int first_pitch_move = indexOf(ev, "MOVE P");
|
|
REQUIRE(first_yaw_move >= 0);
|
|
REQUIRE(first_pitch_move >= 0);
|
|
CHECK(first_yaw_move < first_pitch_move);
|
|
|
|
// No-mag profile switch, drift correction, then heading reset happen AFTER the
|
|
// pitch sweep and BEFORE the yaw sweep reaches its far end.
|
|
const int prof = indexOf(ev, "setProfile:53"); // VRU_general from the fake list
|
|
const int norot = indexOf(ev, "noRotation");
|
|
const int reset = indexOf(ev, "headingReset");
|
|
REQUIRE(prof >= 0);
|
|
REQUIRE(norot >= 0);
|
|
REQUIRE(reset >= 0);
|
|
CHECK(first_pitch_move < prof); // pitch calibrated before the profile switch
|
|
CHECK(prof < norot); // switch to no-mag, then correct drift
|
|
CHECK(norot < reset); // correct drift, then reset
|
|
CHECK(reset < lastIndexOf(ev, "MOVE Y")); // yaw sweep continues after the reset
|
|
|
|
// Pitch is commanded to 0 deg before the heading reset.
|
|
CHECK(indexOf(ev, "MOVE P 0") >= 0);
|
|
CHECK(indexOf(ev, "MOVE P 0") < reset);
|
|
}
|
|
|
|
TEST_CASE("calibration skips the profile switch when no no-mag profile exists") {
|
|
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.has_profiles = false; // device reports no available profiles
|
|
|
|
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
const auto& ev = motor.events;
|
|
CHECK(indexOf(ev, "setProfile") == -1); // nothing to switch to
|
|
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
|
CHECK(cal.report().valid);
|
|
}
|
|
|
|
TEST_CASE("calibration recovers the fake IMU slope for both axes") {
|
|
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);
|
|
|
|
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
CalibReport rep = cal.report();
|
|
REQUIRE(rep.valid);
|
|
CHECK(rep.all_ok);
|
|
REQUIRE(rep.axes.size() == 2);
|
|
for (const auto& ax : rep.axes) {
|
|
CHECK(ax.ok);
|
|
CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001));
|
|
}
|
|
|
|
// The new geometry is published for the session to apply.
|
|
auto geo = cal.takeResult();
|
|
REQUIRE(geo.has_value());
|
|
CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
}
|
|
|
|
TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") {
|
|
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);
|
|
|
|
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
runToCompletion(cal);
|
|
|
|
const auto& ev = motor.events;
|
|
CHECK(indexOf(ev, "MOVE P") == -1); // no pitch moves
|
|
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
|
CHECK(indexOf(ev, "MOVE Y") >= 0);
|
|
|
|
CalibReport rep = cal.report();
|
|
REQUIRE(rep.valid);
|
|
REQUIRE(rep.axes.size() == 1);
|
|
CHECK(rep.axes[0].axis == 'Y');
|
|
CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
}
|