49 lines
2.0 KiB
C++
49 lines
2.0 KiB
C++
#pragma once
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
// Fitting the yaw map from operator sightings of known landmarks.
|
|
//
|
|
// The gimbal is aimed at something whose true bearing is known (a mast, a peak, a
|
|
// road junction on the map), and the encoder position is recorded against that
|
|
// bearing. Two sightings determine the line exactly; more are least-squares
|
|
// fitted. Nothing here needs an IMU, which is the point: not every tower has one,
|
|
// and IMU yaw drifts badly enough that it was never a trustworthy heading source.
|
|
//
|
|
// Kept I/O-free in fgc_core so the fit is unit-testable without hardware.
|
|
|
|
struct HeadingSighting {
|
|
long counts = 0; // yaw encoder position when the landmark was centred
|
|
double heading_deg = 0.0; // its true bearing, degrees (0 = north)
|
|
};
|
|
|
|
struct HeadingFitResult {
|
|
bool ok = false;
|
|
std::string error; // operator-facing reason when !ok
|
|
double counts_per_deg = 0.0;
|
|
double deg_at_center = 0.0; // bearing at center_count, normalised to [0,360)
|
|
double r2 = 0.0;
|
|
int n = 0;
|
|
double span_deg = 0.0; // angular baseline actually used
|
|
};
|
|
|
|
// Shortest baseline we will fit. Two sightings a few degrees apart put the whole
|
|
// scale on a handful of counts, so a small aiming error swings counts_per_deg
|
|
// wildly - and the result looks perfectly healthy (two points always fit exactly,
|
|
// R^2 = 1). Refusing is the only way the operator finds out.
|
|
inline constexpr double kMinHeadingBaselineDeg = 10.0;
|
|
|
|
// Fit counts_per_deg and the bearing at `center_count`. Sightings may be given in
|
|
// any order and may span north.
|
|
HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count);
|
|
|
|
// Parse "<counts>:<deg>" tokens (the form the TUI wizard emits). Returns false and
|
|
// sets `error` on the first malformed token.
|
|
bool parseHeadingSightings(const std::vector<std::string>& tokens,
|
|
std::vector<HeadingSighting>& out, std::string& error);
|
|
|
|
} // namespace fgc
|