34 lines
1.3 KiB
C++
34 lines
1.3 KiB
C++
#pragma once
|
|
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
// Pure math helpers for `gimbal calib`. Kept I/O-free in fgc_core so the fit can
|
|
// be unit-tested independently of the threaded routine that drives the hardware.
|
|
|
|
struct LinearFit {
|
|
bool ok = false; // false if < 2 points or x has no spread
|
|
double slope = 0; // y = slope*x + intercept
|
|
double intercept = 0;
|
|
double r2 = 0; // coefficient of determination (1 = perfect)
|
|
int n = 0;
|
|
};
|
|
|
|
// Least-squares fit of y over x. For calibration: x = measured degrees,
|
|
// y = motor encoder counts ⇒ slope = counts_per_deg, intercept = zero_count.
|
|
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy);
|
|
|
|
// Circular mean of angles in degrees, robust to ±180° wrap (e.g. IMU heading).
|
|
// Returns a value in (-180, 180]. Empty input returns 0.
|
|
double circularMeanDeg(const std::vector<double>& degs);
|
|
|
|
// Phase-unwrap `deg` relative to the previous (already-unwrapped) sample: shift it
|
|
// by whole turns so it lands within ±180° of `prev`. Feeding a smoothly-swept
|
|
// angle through this in sequence removes the 0/360 (or ±180) discontinuity, so the
|
|
// calibration sees a continuous curve for the linear fit.
|
|
double unwrapNear(double prev, double deg);
|
|
|
|
} // namespace fgc
|