81 lines
2.7 KiB
C++
81 lines
2.7 KiB
C++
#include "fgc/ScanGrid.h"
|
|
|
|
#include "fgc/Geometry.h"
|
|
|
|
#include <fstream>
|
|
#include <set>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
std::string trim(const std::string& s) {
|
|
size_t b = s.find_first_not_of(" \t\r\n");
|
|
if (b == std::string::npos) return "";
|
|
size_t e = s.find_last_not_of(" \t\r\n");
|
|
return s.substr(b, e - b + 1);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::vector<ScanPoint> parseScanCsv(const std::string& text) {
|
|
std::vector<ScanPoint> pts;
|
|
std::istringstream in(text);
|
|
std::string line;
|
|
int lineno = 0;
|
|
while (std::getline(in, line)) {
|
|
++lineno;
|
|
std::string t = trim(line);
|
|
if (t.empty() || t[0] == '#') continue;
|
|
// "yaw_deg,pitch_deg", or "yaw_deg" alone for pitch 0 (a yaw-only tower).
|
|
size_t comma = t.find(',');
|
|
try {
|
|
ScanPoint p;
|
|
p.yaw_deg = std::stod(trim(t.substr(0, comma)));
|
|
if (comma != std::string::npos) p.pitch_deg = std::stod(trim(t.substr(comma + 1)));
|
|
pts.push_back(p);
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("scan grid line " + std::to_string(lineno) +
|
|
": non-numeric coordinate in '" + t + "'");
|
|
}
|
|
}
|
|
return pts;
|
|
}
|
|
|
|
std::size_t repeatedBearings(const std::vector<ScanPoint>& points) {
|
|
std::set<double> seen;
|
|
std::size_t repeats = 0;
|
|
for (const auto& p : points)
|
|
if (!seen.insert(wrap360(p.yaw_deg)).second) ++repeats;
|
|
return repeats;
|
|
}
|
|
|
|
ScanGrid ScanGrid::fromConfig(const ScanConfig& cfg) {
|
|
// No grid_file, no grid. Generating a default sweep here would put a tower
|
|
// on the air photographing bearings nobody picked, under a configuration
|
|
// that shows no waypoints at all; refusing leaves auto-sweep off and says
|
|
// exactly what is missing.
|
|
if (cfg.grid_file.empty())
|
|
throw std::runtime_error("no scan grid configured: set [Scan] grid_file to a CSV of "
|
|
"'yaw_deg,pitch_deg' waypoints (see config/scan.csv)");
|
|
std::ifstream f(cfg.grid_file);
|
|
if (!f) throw std::runtime_error("cannot open scan grid file: " + cfg.grid_file);
|
|
std::stringstream buf;
|
|
buf << f.rdbuf();
|
|
auto pts = parseScanCsv(buf.str());
|
|
if (pts.empty()) throw std::runtime_error("scan grid file has no waypoints: " + cfg.grid_file);
|
|
return ScanGrid(std::move(pts));
|
|
}
|
|
|
|
void ScanGrid::advance() {
|
|
if (points_.empty()) return;
|
|
// Wrap rather than reverse: every pass visits the waypoints in the same
|
|
// order, at the cost of one long return slew back to the start.
|
|
index_ = (index_ + 1) % points_.size();
|
|
}
|
|
|
|
} // namespace fgc
|