100 lines
3.7 KiB
C++
100 lines
3.7 KiB
C++
#include "fgc/HeadingFit.h"
|
|
|
|
#include "fgc/Calibration.h"
|
|
#include "fgc/Geometry.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
|
|
namespace fgc {
|
|
|
|
HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count) {
|
|
HeadingFitResult r;
|
|
r.n = static_cast<int>(sightings.size());
|
|
|
|
if (sightings.size() < 2) {
|
|
r.error = "need at least two sightings (aim at a second landmark)";
|
|
return r;
|
|
}
|
|
|
|
// Sort by encoder position so the bearings can be unwrapped along the axis.
|
|
// Sighting order is whatever the operator found convenient; the axis is the
|
|
// only thing that varies monotonically.
|
|
std::vector<HeadingSighting> pts = sightings;
|
|
std::sort(pts.begin(), pts.end(),
|
|
[](const HeadingSighting& a, const HeadingSighting& b) { return a.counts < b.counts; });
|
|
|
|
// Unwrap so a set spanning north fits as a straight line instead of folding
|
|
// at 360 (e.g. 350 and 10 become 350 and 370, not 350 and 10).
|
|
std::vector<std::pair<double, double>> xy; // x = bearing, y = counts
|
|
xy.reserve(pts.size());
|
|
double prev = wrap180(pts.front().heading_deg);
|
|
for (std::size_t i = 0; i < pts.size(); ++i) {
|
|
const double deg = (i == 0) ? prev : unwrapNear(prev, pts[i].heading_deg);
|
|
prev = deg;
|
|
xy.emplace_back(deg, static_cast<double>(pts[i].counts));
|
|
}
|
|
|
|
auto [lo, hi] = std::minmax_element(
|
|
xy.begin(), xy.end(), [](const auto& a, const auto& b) { return a.first < b.first; });
|
|
r.span_deg = hi->first - lo->first;
|
|
if (r.span_deg < kMinHeadingBaselineDeg) {
|
|
r.error = "sightings are only " + std::to_string(static_cast<int>(r.span_deg)) +
|
|
" deg apart; use landmarks at least " +
|
|
std::to_string(static_cast<int>(kMinHeadingBaselineDeg)) +
|
|
" deg apart (further is better)";
|
|
return r;
|
|
}
|
|
|
|
const LinearFit fit = linearFit(xy);
|
|
if (!fit.ok) {
|
|
r.error = "degenerate sightings (all at the same bearing)";
|
|
return r;
|
|
}
|
|
// Distinct bearings at the same encoder position: the axis did not move
|
|
// between sightings, so one of the bearings is wrong.
|
|
if (std::abs(fit.slope) < 1e-6) {
|
|
r.error = "sightings give no encoder travel between bearings; did the gimbal move?";
|
|
return r;
|
|
}
|
|
|
|
r.counts_per_deg = fit.slope;
|
|
// The map is anchored on the homed centre, so what gets stored is the bearing
|
|
// that centre points at - solve the fitted line for it.
|
|
r.deg_at_center = wrap360((static_cast<double>(center_count) - fit.intercept) / fit.slope);
|
|
r.r2 = fit.r2;
|
|
r.ok = true;
|
|
return r;
|
|
}
|
|
|
|
bool parseHeadingSightings(const std::vector<std::string>& tokens,
|
|
std::vector<HeadingSighting>& out, std::string& error) {
|
|
out.clear();
|
|
for (const std::string& t : tokens) {
|
|
const auto colon = t.find(':');
|
|
if (colon == std::string::npos) {
|
|
error = "expected <counts>:<deg>, got '" + t + "'";
|
|
return false;
|
|
}
|
|
HeadingSighting s;
|
|
try {
|
|
std::size_t used = 0;
|
|
s.counts = std::stol(t.substr(0, colon), &used);
|
|
if (used != colon) throw std::invalid_argument("trailing");
|
|
const std::string deg = t.substr(colon + 1);
|
|
s.heading_deg = std::stod(deg, &used);
|
|
if (used != deg.size()) throw std::invalid_argument("trailing");
|
|
} catch (const std::exception&) {
|
|
error = "bad sighting '" + t + "' (expected <counts>:<deg>)";
|
|
return false;
|
|
}
|
|
out.push_back(s);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} // namespace fgc
|