112 lines
3.9 KiB
C++
112 lines
3.9 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 pitch setup, 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 pitch setup (persists for display).
|
|
struct CalibReport {
|
|
struct Axis {
|
|
char axis = '?';
|
|
bool ok = false;
|
|
double counts_per_deg = 0;
|
|
double deg_at_center = 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
|
|
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
|
|
double inset_frac = 0.05; // keep targets off the hard endstops
|
|
};
|
|
|
|
// `gimbal pitch setup`: an IMU-referenced steps<->degrees calibration for the
|
|
// PITCH axis only. Runs on its own thread (the motor/imu/Logger interfaces are
|
|
// thread-safe). Sequence:
|
|
// 1. abort unless the IMU is connected - there is no other way to find level;
|
|
// 2. home the gimbal if it is not already READY;
|
|
// 3. sweep pitch across its soft-limit travel, dwelling at each step to record
|
|
// the gravity-referenced IMU pitch, and least-squares fit counts vs degrees;
|
|
// 4. park at 0 deg (level) using the map just fitted.
|
|
//
|
|
// Yaw is deliberately absent. It is tied to true north by `gimbal heading setup`
|
|
// against landmarks the operator can identify, which needs no IMU (not every
|
|
// tower has one) and does not inherit the MTi's heading drift - the reason the
|
|
// old two-axis routine had to switch XKF profiles and reset the heading mid-run
|
|
// just to get one usable yaw sweep.
|
|
//
|
|
// The resulting Geometry is published via takeResult() for the main thread to
|
|
// apply; raw samples + the fit are written to a logfile. Progress streams to the
|
|
// LOG pane. Cancellable, one-at-a-time.
|
|
class PitchSetupRoutine {
|
|
public:
|
|
PitchSetupRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
|
CalibParams params = {});
|
|
~PitchSetupRoutine();
|
|
|
|
// 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
|