111 lines
4.1 KiB
C++
111 lines
4.1 KiB
C++
#pragma once
|
|
|
|
#include "fgc/Geometry.h"
|
|
|
|
#include <atomic>
|
|
#include <mutex>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
class IMotorController;
|
|
class IImuSource;
|
|
|
|
// Live progress of a running calibration, for the TUI activity strip.
|
|
struct CalibProgress {
|
|
bool running = false;
|
|
char axis = '?'; // 'Y' / 'P'
|
|
int step = 0; // 1-based current step
|
|
int total = 0; // steps per axis
|
|
std::string phase; // "moving" / "dwelling" / "fitting" / ...
|
|
};
|
|
|
|
// Structured outcome of the last completed calibration (persists for display).
|
|
struct CalibReport {
|
|
struct Axis {
|
|
char axis = '?';
|
|
bool ok = false;
|
|
double counts_per_deg = 0;
|
|
long zero_count = 0;
|
|
double r2 = 0;
|
|
int n = 0;
|
|
};
|
|
bool valid = false;
|
|
long long ts_ms = 0; // wall-clock completion time (epoch ms)
|
|
bool all_ok = false;
|
|
std::vector<Axis> axes;
|
|
};
|
|
|
|
// Tunable timings/sizes for a calibration run. Defaults are the production
|
|
// values; tests inject tiny ones so a full run completes in milliseconds.
|
|
struct CalibParams {
|
|
int positions = 10; // sweep steps per axis
|
|
int dwell_ms = 5000; // hold at each position while sampling
|
|
int sample_ms = 100; // IMU sampling cadence during a dwell
|
|
int settle_timeout_ms = 15000; // max wait for a single MOVE to settle
|
|
int home_timeout_ms = 90000; // max wait for HOME to reach READY
|
|
int norotation_s = 3; // no-rotation gyro-bias update duration
|
|
int reset_settle_ms = 1500; // let the filter apply the heading reset
|
|
double inset_frac = 0.05; // keep targets off the hard endstops
|
|
};
|
|
|
|
// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
|
|
// thread (the motor/imu/Logger interfaces are thread-safe). Sequence:
|
|
// 1. home the gimbal if it is not already READY;
|
|
// 2. move yaw to its first sweep position and calibrate PITCH there (sweep its
|
|
// soft-limit travel, dwelling to record the gravity-referenced IMU pitch);
|
|
// 3. move pitch to 0 deg;
|
|
// 4. switch the IMU to a no-magnetometer XKF profile (persists on the device) so
|
|
// its heading stops chasing the stepper-distorted magnetic field;
|
|
// 5. with the gimbal held still, run the IMU no-rotation update (cuts yaw drift)
|
|
// then reset the IMU heading so the current pose is yaw 0 — this removes the
|
|
// drift accumulated during the slow pitch sweep right before it matters;
|
|
// 6. sweep YAW from that first position and calibrate it.
|
|
// Each axis fit is least-squares (counts vs degrees). The resulting Geometry is
|
|
// published via takeResult() for the main thread to apply; raw samples + fits are
|
|
// written to a logfile. Progress streams to the LOG pane. Cancellable, one-at-a-time.
|
|
class CalibrationRoutine {
|
|
public:
|
|
CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
|
CalibParams params = {});
|
|
~CalibrationRoutine();
|
|
|
|
// Begin on a worker thread. Logs a reason and returns false if already
|
|
// running (prechecks happen on the worker and abort there).
|
|
bool start();
|
|
void cancel();
|
|
bool running() const { return running_.load(); }
|
|
|
|
// If a finished run produced a new calibration, returns it once (then clears).
|
|
std::optional<Geometry> takeResult();
|
|
|
|
// Live progress (thread-safe copy) and the last completed report (persists).
|
|
CalibProgress progress() const;
|
|
CalibReport report() const;
|
|
|
|
private:
|
|
void run();
|
|
void setProgress(char axis, int step, int total, const char* phase);
|
|
|
|
IMotorController& motor_;
|
|
IImuSource& imu_;
|
|
Geometry initial_;
|
|
CalibParams params_;
|
|
|
|
std::thread thread_;
|
|
std::atomic<bool> running_{false};
|
|
std::atomic<bool> cancel_{false};
|
|
|
|
mutable std::mutex result_mutex_;
|
|
std::optional<Geometry> result_;
|
|
CalibReport report_;
|
|
|
|
mutable std::mutex progress_mutex_;
|
|
CalibProgress progress_;
|
|
};
|
|
|
|
} // namespace fgc
|