1026 lines
51 KiB
C++
1026 lines
51 KiB
C++
#include "fgc/Config.h"
|
|
#include "fgc/FirmwareConfig.h"
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/Paths.h"
|
|
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include <cctype>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
|
|
extern "C" {
|
|
#include "ini.h"
|
|
}
|
|
|
|
namespace fgc {
|
|
|
|
namespace fs = std::filesystem;
|
|
|
|
namespace {
|
|
|
|
std::string get(const std::map<std::string, std::string>& kv, const std::string& key,
|
|
const std::string& fallback = "") {
|
|
auto it = kv.find(key);
|
|
return (it != kv.end() && !it->second.empty()) ? it->second : fallback;
|
|
}
|
|
|
|
int getInt(const std::map<std::string, std::string>& kv, const std::string& key, int fallback) {
|
|
auto it = kv.find(key);
|
|
if (it == kv.end() || it->second.empty()) return fallback;
|
|
try {
|
|
// Reject trailing characters: bare std::stoi stops at the first non-digit,
|
|
// so "1883x" would quietly become 1883 and a typo'd value would configure
|
|
// something the operator never wrote. Same reasoning as getIntAutoBase below.
|
|
size_t consumed = 0;
|
|
int out = std::stoi(it->second, &consumed);
|
|
if (consumed != it->second.size()) throw std::invalid_argument("trailing characters");
|
|
return out;
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("Config key '" + key + "' must be an integer, got '" +
|
|
it->second + "'");
|
|
}
|
|
}
|
|
|
|
// Like getInt, but accepts an "0x"-prefixed value too. Only for I2C addresses,
|
|
// which every datasheet, `i2cdetect` and our own docs write in hex: plain
|
|
// std::stoi() stops at the 'x' and silently yields 0, so `i2c_addr = 0x44`
|
|
// would configure address 0x00 and the sensor would never answer. Kept separate
|
|
// from getInt so no other key gains surprise octal ("010" -> 8) parsing.
|
|
int getIntAutoBase(const std::map<std::string, std::string>& kv, const std::string& key,
|
|
int fallback) {
|
|
auto it = kv.find(key);
|
|
if (it == kv.end() || it->second.empty()) return fallback;
|
|
try {
|
|
size_t consumed = 0;
|
|
const std::string& v = it->second;
|
|
int out = std::stoi(v, &consumed, v.rfind("0x", 0) == 0 || v.rfind("0X", 0) == 0 ? 16 : 10);
|
|
if (consumed != v.size()) throw std::invalid_argument("trailing characters");
|
|
return out;
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("Config key '" + key + "' must be an integer (decimal or 0x hex)"
|
|
", got '" + it->second + "'");
|
|
}
|
|
}
|
|
|
|
double getDouble(const std::map<std::string, std::string>& kv, const std::string& key,
|
|
double fallback) {
|
|
auto it = kv.find(key);
|
|
if (it == kv.end() || it->second.empty()) return fallback;
|
|
try {
|
|
return std::stod(it->second);
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("Config key '" + key + "' must be a number, got '" +
|
|
it->second + "'");
|
|
}
|
|
}
|
|
|
|
long getLong(const std::map<std::string, std::string>& kv, const std::string& key, long fallback) {
|
|
auto it = kv.find(key);
|
|
if (it == kv.end() || it->second.empty()) return fallback;
|
|
try {
|
|
return std::stol(it->second);
|
|
} catch (const std::exception&) {
|
|
throw std::runtime_error("Config key '" + key + "' must be an integer, got '" +
|
|
it->second + "'");
|
|
}
|
|
}
|
|
|
|
bool getBool(const std::map<std::string, std::string>& kv, const std::string& key, bool fallback) {
|
|
auto it = kv.find(key);
|
|
if (it == kv.end() || it->second.empty()) return fallback;
|
|
const std::string& v = it->second;
|
|
if (v == "1" || v == "true" || v == "yes" || v == "on") return true;
|
|
if (v == "0" || v == "false" || v == "no" || v == "off") return false;
|
|
throw std::runtime_error("Config key '" + key + "' must be a boolean, got '" + v + "'");
|
|
}
|
|
|
|
// "yaw, pitch" -> {"yaw","pitch"}. Lowercased and trimmed; empty entries dropped,
|
|
// so trailing commas and stray spacing are not errors an operator has to hunt.
|
|
std::vector<std::string> splitList(const std::string& s) {
|
|
std::vector<std::string> out;
|
|
std::string cur;
|
|
auto flush = [&] {
|
|
size_t a = cur.find_first_not_of(" \t");
|
|
if (a == std::string::npos) { cur.clear(); return; }
|
|
size_t b = cur.find_last_not_of(" \t");
|
|
std::string t = cur.substr(a, b - a + 1);
|
|
for (char& c : t) c = static_cast<char>(std::tolower((unsigned char)c));
|
|
if (!t.empty()) out.push_back(t);
|
|
cur.clear();
|
|
};
|
|
for (char c : s) {
|
|
if (c == ',') flush();
|
|
else cur += c;
|
|
}
|
|
flush();
|
|
return out;
|
|
}
|
|
|
|
std::string lowered(std::string s) {
|
|
for (char& c : s) c = static_cast<char>(std::tolower((unsigned char)c));
|
|
return s;
|
|
}
|
|
|
|
std::string envOr(const char* name, const std::string& fallback) {
|
|
const char* v = std::getenv(name);
|
|
return (v && *v) ? std::string(v) : fallback;
|
|
}
|
|
|
|
// inih callback: flatten "Section.name" => value into the map.
|
|
int iniHandler(void* user, const char* section, const char* name, const char* value) {
|
|
auto* kv = static_cast<std::map<std::string, std::string>*>(user);
|
|
(*kv)[std::string(section) + "." + std::string(name)] = value ? value : "";
|
|
return 1;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
CameraProfileTiming CameraSpec::timingFor(const std::string& profile_name) const {
|
|
auto it = timing.find(profile_name);
|
|
return it != timing.end() ? it->second : CameraProfileTiming{};
|
|
}
|
|
|
|
std::vector<const CameraSpec*> CameraConfig::active() const {
|
|
std::vector<const CameraSpec*> out;
|
|
for (const auto& c : cameras)
|
|
if (c.enabled) out.push_back(&c);
|
|
return out;
|
|
}
|
|
|
|
int CameraConfig::maxSettleWaitMs() const {
|
|
int worst = 0;
|
|
for (const auto* c : active()) worst = std::max(worst, c->timingFor(c->profile).settle_wait_ms);
|
|
return worst > 0 ? worst : CameraProfileTiming{}.settle_wait_ms;
|
|
}
|
|
|
|
int CameraConfig::maxFrameTimeoutMs() const {
|
|
int worst = 0;
|
|
for (const auto* c : active())
|
|
worst = std::max(worst, c->timingFor(c->profile).frame_timeout_ms);
|
|
return worst > 0 ? worst : CameraProfileTiming{}.frame_timeout_ms;
|
|
}
|
|
|
|
std::string NetworkConfig::brokerUri() const {
|
|
// A scheme means the operator wrote the whole URI: hands off. Appending a port
|
|
// could corrupt one that already has it, or a ws:// path.
|
|
if (broker_ip.find("://") != std::string::npos) return broker_ip;
|
|
|
|
std::string host = broker_ip;
|
|
bool has_port = false;
|
|
if (!host.empty() && host.front() == '[') { // bracketed IPv6
|
|
const size_t close = host.rfind(']');
|
|
has_port = close != std::string::npos && host.find(':', close) != std::string::npos;
|
|
} else {
|
|
const size_t colons = static_cast<size_t>(std::count(host.begin(), host.end(), ':'));
|
|
if (colons == 1) {
|
|
has_port = true;
|
|
} else if (colons > 1) {
|
|
host = "[" + host + "]"; // bare IPv6; bracket it so the port is unambiguous
|
|
}
|
|
}
|
|
|
|
if (has_port) return "tcp://" + host;
|
|
return "tcp://" + host + ":" + std::to_string(mqtt_port);
|
|
}
|
|
|
|
double AppConfig::image_rate() const {
|
|
return general.image_interval > 0 ? 1.0 / general.image_interval : 0.0;
|
|
}
|
|
|
|
double TestProfile::get(const std::string& key, double fallback) const {
|
|
auto it = params.find(key);
|
|
return it != params.end() ? it->second : fallback;
|
|
}
|
|
|
|
int TestProfile::geti(const std::string& key, int fallback) const {
|
|
auto it = params.find(key);
|
|
return it != params.end() ? static_cast<int>(it->second) : fallback;
|
|
}
|
|
|
|
bool TestProfile::getBool(const std::string& key, bool fallback) const {
|
|
auto it = text.find(key);
|
|
if (it == text.end() || it->second.empty()) return fallback;
|
|
const std::string& v = it->second;
|
|
return v == "1" || v == "true" || v == "yes" || v == "on";
|
|
}
|
|
|
|
std::string TestProfile::gets(const std::string& key, const std::string& fallback) const {
|
|
auto it = text.find(key);
|
|
return (it != text.end() && !it->second.empty()) ? it->second : fallback;
|
|
}
|
|
|
|
std::vector<long> TestProfile::getLongList(const std::string& key) const {
|
|
std::vector<long> out;
|
|
auto it = text.find(key);
|
|
if (it == text.end()) return out;
|
|
std::stringstream ss(it->second);
|
|
std::string tok;
|
|
while (std::getline(ss, tok, ',')) {
|
|
try { out.push_back(std::stol(tok)); } catch (...) {}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
const TestProfile* TestConfig::find(const std::string& name) const {
|
|
auto it = profiles.find(name);
|
|
return it != profiles.end() ? &it->second : nullptr;
|
|
}
|
|
|
|
AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
|
|
AppConfig cfg;
|
|
|
|
cfg.general.tower_name = get(kv, "General.tower_name", cfg.general.tower_name);
|
|
cfg.general.image_interval = getDouble(kv, "General.image_interval", cfg.general.image_interval);
|
|
cfg.general.debug = getBool(kv, "General.debug", cfg.general.debug);
|
|
|
|
// [Board] / [Hardware]: normally from hardware.ini, but read from the same
|
|
// merged map, so a config.ini carrying them still works during migration.
|
|
cfg.board.model = get(kv, "Board.model", cfg.board.model);
|
|
cfg.board.firmware_env = get(kv, "Board.firmware_env", cfg.board.firmware_env);
|
|
|
|
cfg.hardware.axes = splitList(get(kv, "Hardware.axes"));
|
|
cfg.hardware.cameras = splitList(get(kv, "Hardware.cameras"));
|
|
cfg.hardware.imu = lowered(get(kv, "Hardware.imu"));
|
|
cfg.hardware.env = lowered(get(kv, "Hardware.env"));
|
|
cfg.hardware.cooling = lowered(get(kv, "Hardware.cooling"));
|
|
// The same rules the firmware build applies (firmware/tools/gen_wiring.py):
|
|
// the fan is run from the DHT11's temperature, on the MCU.
|
|
if (!cfg.hardware.cooling.empty() && cfg.hardware.cooling != "none" &&
|
|
cfg.hardware.cooling != "fan")
|
|
throw std::runtime_error("Hardware.cooling must be 'none' or 'fan' (got '" +
|
|
cfg.hardware.cooling + "')");
|
|
if (cfg.hardware.cooling == "fan" && cfg.hardware.env != "dht11")
|
|
throw std::runtime_error("Hardware.cooling = fan needs Hardware.env = dht11: the fan "
|
|
"is driven from the temperature the motor MCU reads");
|
|
{
|
|
auto& p = cfg.hardware.pins;
|
|
p.yaw_cs = getInt(kv, "Pins.yaw_cs", -1);
|
|
p.yaw_en = getInt(kv, "Pins.yaw_en", -1);
|
|
p.pitch_cs = getInt(kv, "Pins.pitch_cs", -1);
|
|
p.pitch_en = getInt(kv, "Pins.pitch_en", -1);
|
|
p.spi_sck = getInt(kv, "Pins.spi_sck", -1);
|
|
p.spi_mosi = getInt(kv, "Pins.spi_mosi", -1);
|
|
p.spi_miso = getInt(kv, "Pins.spi_miso", -1);
|
|
p.dht = getInt(kv, "Pins.dht", -1);
|
|
p.fan_pwm = getInt(kv, "Pins.fan_pwm", -1);
|
|
p.fan_tach = getInt(kv, "Pins.fan_tach", -1);
|
|
p.spi_idle_cs = getInt(kv, "Pins.spi_idle_cs", -1);
|
|
}
|
|
if (kv.count("Hardware.pan_step_deg")) {
|
|
cfg.hardware.pan_step_deg = getDouble(kv, "Hardware.pan_step_deg", 0.0);
|
|
if (!(cfg.hardware.pan_step_deg > 0.0))
|
|
throw std::runtime_error("Hardware.pan_step_deg must be > 0 (degrees of bearing "
|
|
"one picture covers)");
|
|
}
|
|
cfg.hardware.save_workers = getInt(kv, "Hardware.save_workers", cfg.hardware.save_workers);
|
|
cfg.hardware.jxl_threads = getInt(kv, "Hardware.jxl_threads", cfg.hardware.jxl_threads);
|
|
cfg.hardware.save_queue_max = getInt(kv, "Hardware.save_queue_max", cfg.hardware.save_queue_max);
|
|
if (cfg.hardware.save_workers < 1 || cfg.hardware.save_workers > 4)
|
|
throw std::runtime_error("Hardware.save_workers must be in 1..4 (the boards have 4 cores)");
|
|
if (cfg.hardware.jxl_threads < 0 || cfg.hardware.jxl_threads > 8)
|
|
throw std::runtime_error("Hardware.jxl_threads must be in 0..8 (0 = 4 / save_workers)");
|
|
if (cfg.hardware.jxl_threads == 0)
|
|
cfg.hardware.jxl_threads = std::max(1, 4 / cfg.hardware.save_workers);
|
|
if (cfg.hardware.save_queue_max < 1 || cfg.hardware.save_queue_max > 32)
|
|
throw std::runtime_error("Hardware.save_queue_max must be in 1..32 (each raw frame "
|
|
"waiting holds ~24 MB)");
|
|
// [Thermal]: the CPU guard (ThermalGuard.h). Per computer, so normally in
|
|
// hardware.ini; config.ini works too (hardware.ini wins).
|
|
{
|
|
ThermalParams& t = cfg.thermal;
|
|
t.enabled = getBool(kv, "Thermal.enabled", t.enabled);
|
|
t.sysfs_root = get(kv, "Thermal.sysfs_root", t.sysfs_root);
|
|
if (t.sysfs_root.empty()) t.sysfs_root = "/";
|
|
t.sample_ms = getInt(kv, "Thermal.sample_ms", t.sample_ms);
|
|
t.slow_c = getDouble(kv, "Thermal.slow_c", t.slow_c);
|
|
t.pause_c = getDouble(kv, "Thermal.pause_c", t.pause_c);
|
|
t.resume_c = getDouble(kv, "Thermal.resume_c", t.resume_c);
|
|
t.hysteresis_c = getDouble(kv, "Thermal.hysteresis_c", t.hysteresis_c);
|
|
t.pause_after_s = getInt(kv, "Thermal.pause_after_s", t.pause_after_s);
|
|
t.calm_s = getInt(kv, "Thermal.calm_s", t.calm_s);
|
|
t.min_pause_s = getInt(kv, "Thermal.min_pause_s", t.min_pause_s);
|
|
t.held_slow_s = getInt(kv, "Thermal.held_slow_s", t.held_slow_s);
|
|
t.held_pause_s = getInt(kv, "Thermal.held_pause_s", t.held_pause_s);
|
|
t.slow_factor = getDouble(kv, "Thermal.slow_factor", t.slow_factor);
|
|
t.held_share_pct = getDouble(kv, "Thermal.held_share_pct", t.held_share_pct);
|
|
if (t.held_share_pct <= 0.0 || t.held_share_pct > 100.0)
|
|
throw std::runtime_error("Thermal.held_share_pct must be in 0..100 (exclusive of 0)");
|
|
t.clock_low_pct = getDouble(kv, "Thermal.clock_low_pct", t.clock_low_pct);
|
|
t.busy_high_pct = getDouble(kv, "Thermal.busy_high_pct", t.busy_high_pct);
|
|
if (t.sample_ms < 500 || t.sample_ms > 60000)
|
|
throw std::runtime_error("Thermal.sample_ms must be in 500..60000");
|
|
if (!(t.resume_c < t.slow_c && t.slow_c < t.pause_c))
|
|
throw std::runtime_error("Thermal: need resume_c < slow_c < pause_c (degrees C)");
|
|
if (t.hysteresis_c < 0.0)
|
|
throw std::runtime_error("Thermal.hysteresis_c must be >= 0");
|
|
if (t.slow_factor < 1.0 || t.slow_factor > 10.0)
|
|
throw std::runtime_error("Thermal.slow_factor must be in 1..10 (times the interval)");
|
|
if (t.pause_after_s < 0 || t.calm_s < 0 || t.min_pause_s < 0 || t.held_slow_s < 1 ||
|
|
t.held_pause_s < 1)
|
|
throw std::runtime_error("Thermal: the *_s durations must be >= 0 (held_*_s >= 1)");
|
|
if (t.held_pause_s < t.held_slow_s)
|
|
throw std::runtime_error("Thermal: held_pause_s must be >= held_slow_s");
|
|
if (t.clock_low_pct <= 0.0 || t.clock_low_pct >= 100.0 || t.busy_high_pct < 0.0 ||
|
|
t.busy_high_pct > 100.0)
|
|
throw std::runtime_error("Thermal: clock_low_pct must be in 0..100 (exclusive), "
|
|
"busy_high_pct in 0..100");
|
|
}
|
|
|
|
for (const auto& a : cfg.hardware.axes)
|
|
if (a != "yaw" && a != "pitch")
|
|
throw std::runtime_error("Hardware.axes: unknown axis '" + a +
|
|
"' (expected yaw and/or pitch)");
|
|
|
|
cfg.network.broker_ip = get(kv, "Network.zkms_server_ip", cfg.network.broker_ip);
|
|
cfg.network.mqtt_port = getInt(kv, "Network.mqtt_port", cfg.network.mqtt_port);
|
|
if (cfg.network.mqtt_port < 1 || cfg.network.mqtt_port > 65535)
|
|
throw std::runtime_error("Network.mqtt_port must be in 1..65535");
|
|
// Secrets: environment variables win over the file.
|
|
cfg.network.mqtt_user = envOr("FGC_MQTT_USER", get(kv, "Network.mqtt_user"));
|
|
cfg.network.mqtt_pw = envOr("FGC_MQTT_PW", get(kv, "Network.mqtt_pw"));
|
|
|
|
cfg.serial.device = get(kv, "Serial.device", cfg.serial.device);
|
|
cfg.serial.baud = static_cast<unsigned>(getInt(kv, "Serial.baud", cfg.serial.baud));
|
|
|
|
// ---- [Camera] shared defaults, then one [Camera.<name>] block per camera ----
|
|
cfg.camera.jxl_distance = getDouble(kv, "Camera.jxl_distance", cfg.camera.jxl_distance);
|
|
cfg.camera.jxl_effort = getInt(kv, "Camera.jxl_effort", cfg.camera.jxl_effort);
|
|
cfg.camera.demosaic = lowered(get(kv, "Camera.demosaic", cfg.camera.demosaic));
|
|
|
|
// Cameras are discovered from the key names rather than a list, so there is
|
|
// no registry to keep in sync: "Camera.rgb.id" declares a camera called rgb.
|
|
// A third dotted segment ("Camera.rgb.standard.settle_wait_ms") is per-profile
|
|
// timing, so only two-segment suffixes name a camera.
|
|
std::vector<std::string> names; // in first-seen order
|
|
for (const auto& [key, unused] : kv) {
|
|
(void)unused;
|
|
if (key.rfind("Camera.", 0) != 0) continue;
|
|
const std::string rest = key.substr(7);
|
|
const auto dot = rest.find('.');
|
|
if (dot == std::string::npos) continue; // [Camera] itself
|
|
const std::string name = rest.substr(0, dot);
|
|
if (rest.find('.', dot + 1) != std::string::npos) { // per-profile timing
|
|
if (std::find(names.begin(), names.end(), name) == names.end()) names.push_back(name);
|
|
continue;
|
|
}
|
|
if (std::find(names.begin(), names.end(), name) == names.end()) names.push_back(name);
|
|
}
|
|
|
|
cfg.camera.cameras.clear();
|
|
for (const auto& name : names) {
|
|
const std::string pre = "Camera." + name + ".";
|
|
CameraSpec c;
|
|
c.name = name;
|
|
c.enabled = getBool(kv, pre + "enabled", true);
|
|
c.driver = get(kv, pre + "driver", c.driver);
|
|
c.id = get(kv, pre + "id");
|
|
c.label = get(kv, pre + "label", name);
|
|
c.profile_dir = get(kv, pre + "profile_dir");
|
|
c.profile = get(kv, pre + "profile", c.profile);
|
|
c.rotation_deg = getInt(kv, pre + "rotation_deg", c.rotation_deg);
|
|
c.jxl_distance = getDouble(kv, pre + "jxl_distance", cfg.camera.jxl_distance);
|
|
c.jxl_effort = getInt(kv, pre + "jxl_effort", cfg.camera.jxl_effort);
|
|
c.demosaic = lowered(get(kv, pre + "demosaic", cfg.camera.demosaic));
|
|
|
|
// Per-profile timing: Camera.<name>.<profile>.settle_wait_ms
|
|
for (const auto& [key, val] : kv) {
|
|
(void)val;
|
|
if (key.rfind(pre, 0) != 0) continue;
|
|
const std::string rest = key.substr(pre.size());
|
|
const auto dot = rest.find('.');
|
|
if (dot == std::string::npos) continue;
|
|
const std::string profile = rest.substr(0, dot);
|
|
const std::string field = rest.substr(dot + 1);
|
|
CameraProfileTiming& t = c.timing[profile];
|
|
if (field == "settle_wait_ms") t.settle_wait_ms = getInt(kv, key, t.settle_wait_ms);
|
|
else if (field == "frame_timeout_ms") t.frame_timeout_ms = getInt(kv, key, t.frame_timeout_ms);
|
|
else if (field == "settle_rule") t.settle_rule = lowered(get(kv, key));
|
|
else throw std::runtime_error("unknown key '" + key + "' (expected " + pre + profile +
|
|
".settle_wait_ms, .frame_timeout_ms or .settle_rule)");
|
|
if (t.settle_rule != "fixed" && t.settle_rule != "brightness")
|
|
throw std::runtime_error(key.substr(0, key.rfind('.')) + ".settle_rule must be "
|
|
"'fixed' or 'brightness'");
|
|
}
|
|
|
|
if (c.driver != "vimba")
|
|
throw std::runtime_error("Camera." + name + ".driver must be 'vimba' (got '" +
|
|
c.driver + "'); no other backend is implemented yet");
|
|
if (c.enabled && c.id.empty())
|
|
throw std::runtime_error("Camera." + name + ".id is required (USB DEV_ id or GigE IP)");
|
|
if (c.profile_dir.empty())
|
|
throw std::runtime_error("Camera." + name + ".profile_dir is required");
|
|
if (c.jxl_distance < 0.0)
|
|
throw std::runtime_error("Camera." + name + ".jxl_distance must be >= 0 (0 = lossless)");
|
|
if (c.jxl_effort < 1 || c.jxl_effort > 9)
|
|
throw std::runtime_error("Camera." + name + ".jxl_effort must be in 1..9");
|
|
if (c.demosaic != "edge_aware" && c.demosaic != "bilinear")
|
|
throw std::runtime_error("Camera." + name + ".demosaic must be 'edge_aware' or "
|
|
"'bilinear' (got '" + c.demosaic + "')");
|
|
if (c.rotation_deg != 0 && c.rotation_deg != 90 && c.rotation_deg != 180 &&
|
|
c.rotation_deg != 270)
|
|
throw std::runtime_error("Camera." + name + ".rotation_deg must be 0, 90, 180 or 270");
|
|
for (const auto& [pname, t] : c.timing) {
|
|
if (t.settle_wait_ms < 0 || t.frame_timeout_ms < 0)
|
|
throw std::runtime_error("Camera." + name + "." + pname +
|
|
" timings must be >= 0");
|
|
}
|
|
cfg.camera.cameras.push_back(std::move(c));
|
|
}
|
|
|
|
std::string out = get(kv, "Paths.output_dir");
|
|
cfg.paths.output_dir = out.empty() ? paths::defaultOutputDir() : paths::expandUser(out);
|
|
|
|
cfg.features.enable_mqtt = getBool(kv, "Features.enable_mqtt", cfg.features.enable_mqtt);
|
|
cfg.features.enable_camera = getBool(kv, "Features.enable_camera", cfg.features.enable_camera);
|
|
cfg.features.enable_serial = getBool(kv, "Features.enable_serial", cfg.features.enable_serial);
|
|
cfg.features.enable_imu = getBool(kv, "Features.enable_imu", cfg.features.enable_imu);
|
|
cfg.features.mock_camera = getBool(kv, "Features.mock_camera", cfg.features.mock_camera);
|
|
{
|
|
const std::string size = get(kv, "Features.mock_camera_size", "small");
|
|
if (size != "small" && size != "full")
|
|
throw std::runtime_error("Features.mock_camera_size must be small or full");
|
|
cfg.features.mock_camera_full = size == "full";
|
|
}
|
|
cfg.features.mock_serial = getBool(kv, "Features.mock_serial", cfg.features.mock_serial);
|
|
cfg.features.mock_imu = getBool(kv, "Features.mock_imu", cfg.features.mock_imu);
|
|
cfg.features.enable_env = getBool(kv, "Features.enable_env", cfg.features.enable_env);
|
|
cfg.features.mock_env = getBool(kv, "Features.mock_env", cfg.features.mock_env);
|
|
cfg.features.mock_needcfg = getBool(kv, "Features.mock_needcfg", cfg.features.mock_needcfg);
|
|
cfg.features.strict_presence =
|
|
getBool(kv, "Features.strict_presence", cfg.features.strict_presence);
|
|
|
|
// [Hardware] imu / env are the declaration; enable_imu / enable_env are the
|
|
// older flags that said it a second time. Resolve them into one answer here,
|
|
// once, so nothing downstream has to know there were ever two keys - and so a
|
|
// rig cannot declare a sensor and then be told it is disabled.
|
|
{
|
|
auto settle = [&](const char* what, const std::string& declared, const char* flag_key,
|
|
bool& out) {
|
|
const SensorEnable e =
|
|
resolveSensorEnable(declared, out, kv.count(flag_key) > 0);
|
|
if (e.conflict)
|
|
LOG_WARN << "[Hardware] " << what << " = '" << declared << "' and [" << flag_key
|
|
<< "] disagree; the declaration wins (" << what << " "
|
|
<< (e.enabled ? "enabled" : "disabled")
|
|
<< ") -- delete the flag, it is what hardware.ini is for";
|
|
else if (e.source == SensorEnableSource::Declaration && kv.count(flag_key))
|
|
LOG_INFO << "[" << flag_key << "] is redundant: [Hardware] " << what
|
|
<< " already declares it";
|
|
out = e.enabled;
|
|
};
|
|
settle("imu", cfg.hardware.imu, "Features.enable_imu", cfg.features.enable_imu);
|
|
settle("env", cfg.hardware.env, "Features.enable_env", cfg.features.enable_env);
|
|
}
|
|
|
|
cfg.imu.device = get(kv, "IMU.device", cfg.imu.device);
|
|
cfg.imu.baud = static_cast<unsigned>(getInt(kv, "IMU.baud", cfg.imu.baud));
|
|
|
|
// [IMU] x/y/z: how the unit is bolted on, in degrees about forward/right/down.
|
|
// A value outside +/-360 is a typo far more often than an intent, and one that
|
|
// is accepted silently is invisible afterwards - the readings just quietly
|
|
// describe the wrong axes.
|
|
cfg.imu.mount.x_deg = getDouble(kv, "IMU.x", cfg.imu.mount.x_deg);
|
|
cfg.imu.mount.y_deg = getDouble(kv, "IMU.y", cfg.imu.mount.y_deg);
|
|
cfg.imu.mount.z_deg = getDouble(kv, "IMU.z", cfg.imu.mount.z_deg);
|
|
for (const auto& [axis, value] : {std::pair{"x", cfg.imu.mount.x_deg},
|
|
std::pair{"y", cfg.imu.mount.y_deg},
|
|
std::pair{"z", cfg.imu.mount.z_deg}}) {
|
|
if (!std::isfinite(value) || std::abs(value) > 360.0)
|
|
throw std::runtime_error(std::string("IMU.") + axis +
|
|
" must be a rotation in degrees within +/-360");
|
|
}
|
|
|
|
cfg.env.i2c_device = get(kv, "Env.i2c_device", cfg.env.i2c_device);
|
|
cfg.env.i2c_addr = static_cast<unsigned>(
|
|
getIntAutoBase(kv, "Env.i2c_addr", static_cast<int>(cfg.env.i2c_addr)));
|
|
cfg.env.period_ms = getInt(kv, "Env.period_ms", cfg.env.period_ms);
|
|
|
|
// [Test] + [TestProfile.<name>]: discover profiles by scanning the flattened
|
|
// keys. inih gives us "TestProfile.<name>.<key>" => value; every numeric value
|
|
// lands in that profile's open param map for the test modules to read.
|
|
cfg.test.default_profile = get(kv, "Test.profile", cfg.test.default_profile);
|
|
cfg.test.baseline_file = get(kv, "Test.baseline_file");
|
|
cfg.test.usbfs_min_mb = getInt(kv, "Test.usbfs_min_mb", cfg.test.usbfs_min_mb);
|
|
cfg.test.usb3_lpm_path = get(kv, "Test.usb3_lpm_path");
|
|
if (!cfg.test.baseline_file.empty())
|
|
cfg.test.baseline_file = paths::expandUser(cfg.test.baseline_file);
|
|
{
|
|
const std::string prefix = "TestProfile.";
|
|
for (const auto& [key, val] : kv) {
|
|
if (key.rfind(prefix, 0) != 0) continue;
|
|
auto dot = key.find('.', prefix.size());
|
|
if (dot == std::string::npos) continue;
|
|
std::string pname = key.substr(prefix.size(), dot - prefix.size());
|
|
std::string pkey = key.substr(dot + 1);
|
|
if (pname.empty() || pkey.empty() || val.empty()) continue;
|
|
auto& prof = cfg.test.profiles[pname];
|
|
prof.name = pname;
|
|
prof.text[pkey] = val; // raw string (covers comma lists)
|
|
try {
|
|
prof.params[pkey] = std::stod(val); // also numeric where possible
|
|
} catch (...) {
|
|
// list/non-numeric value: the raw string in `text` is what modules read
|
|
}
|
|
}
|
|
}
|
|
|
|
cfg.logging.level = get(kv, "Logging.level", cfg.logging.level);
|
|
cfg.logging.trace = get(kv, "Logging.trace", cfg.logging.trace);
|
|
cfg.logging.file = getBool(kv, "Logging.file", cfg.logging.file);
|
|
cfg.logging.file_level = get(kv, "Logging.file_level", cfg.logging.file_level);
|
|
cfg.logging.file_max_mb = getInt(kv, "Logging.file_max_mb", cfg.logging.file_max_mb);
|
|
cfg.logging.file_keep = getInt(kv, "Logging.file_keep", cfg.logging.file_keep);
|
|
cfg.logging.file_dir = get(kv, "Logging.file_dir", cfg.logging.file_dir);
|
|
// Clamp rather than refuse: a silly retention value should not stop a tower
|
|
// starting, but it must not be obeyed either. A max_mb under 1 would rotate
|
|
// on almost every line.
|
|
if (cfg.logging.file_max_mb < 1) cfg.logging.file_max_mb = 1;
|
|
if (cfg.logging.file_max_mb > 1024) cfg.logging.file_max_mb = 1024;
|
|
if (cfg.logging.file_keep < 1) cfg.logging.file_keep = 1;
|
|
if (cfg.logging.file_keep > 100) cfg.logging.file_keep = 100;
|
|
|
|
cfg.ui.enable_tui = getBool(kv, "UI.enable_tui", cfg.ui.enable_tui);
|
|
|
|
cfg.startup.home = getBool(kv, "Startup.home", cfg.startup.home);
|
|
if (auto it = kv.find("Startup.mode"); it != kv.end() && !it->second.empty()) {
|
|
if (it->second == "manual") cfg.startup.mode = Mode::Manual;
|
|
else if (it->second == "auto") cfg.startup.mode = Mode::Auto;
|
|
else
|
|
throw std::runtime_error("Config key 'Startup.mode' must be manual or auto, got '" +
|
|
it->second + "'" +
|
|
(it->second == "pan" ? " (pan is chosen by command: "
|
|
"`mode pan <heading> <span>`)"
|
|
: ""));
|
|
}
|
|
|
|
// [Motor]: the degrees<->counts maps, anchored on the homed mechanical centre
|
|
// (see Geometry). min_deg/max_deg are NOT configured - they are derived from
|
|
// the firmware's homed soft limits, the only place they can be correct.
|
|
cfg.geometry.yaw.counts_per_deg = getDouble(kv, "Motor.yaw_counts_per_deg", cfg.geometry.yaw.counts_per_deg);
|
|
cfg.geometry.yaw.center_count = getLong(kv, "Motor.yaw_center_count", cfg.geometry.yaw.center_count);
|
|
cfg.geometry.yaw.deg_at_center = getDouble(kv, "Motor.yaw_heading_at_center", cfg.geometry.yaw.deg_at_center);
|
|
cfg.geometry.pitch.counts_per_deg = getDouble(kv, "Motor.pitch_counts_per_deg", cfg.geometry.pitch.counts_per_deg);
|
|
cfg.geometry.pitch.center_count = getLong(kv, "Motor.pitch_center_count", cfg.geometry.pitch.center_count);
|
|
cfg.geometry.pitch.deg_at_center = getDouble(kv, "Motor.pitch_angle_at_center", cfg.geometry.pitch.deg_at_center);
|
|
cfg.yaw_soft_margin_counts = getLong(kv, "Motor.yaw_soft_margin_counts", cfg.yaw_soft_margin_counts);
|
|
cfg.pitch_soft_margin_counts = getLong(kv, "Motor.pitch_soft_margin_counts", cfg.pitch_soft_margin_counts);
|
|
cfg.enc_error_warn_counts = getLong(kv, "Motor.enc_error_warn_counts",
|
|
cfg.enc_error_warn_counts);
|
|
cfg.homing_timeout_s = getInt(kv, "Motor.homing_timeout_s", cfg.homing_timeout_s);
|
|
|
|
// [Motor.<axis>]: this tower's drivetrain, normally from hardware.ini. Note
|
|
// the section is DOTTED - these are deliberately different keys from the flat
|
|
// [Motor] ones above, because hardware.ini is merged over config.ini and
|
|
// wins, so a shared key would silently discard the calibration that the save
|
|
// path writes back to config.ini.
|
|
auto axisDrivetrain = [&kv](const char* axis, AxisDrivetrainConfig& d) {
|
|
const std::string pre = std::string("Motor.") + axis + ".";
|
|
for (const auto& [k, v] : kv) {
|
|
(void)v;
|
|
if (k.rfind(pre, 0) == 0) { d.declared = true; break; }
|
|
}
|
|
d.r_sense_ohm = getDouble(kv, pre + "r_sense_ohm", d.r_sense_ohm);
|
|
d.run_current_ma = getDouble(kv, pre + "run_current_ma", d.run_current_ma);
|
|
// An axis that names only a run current is not asking for a reduction.
|
|
d.homing_current_ma = getDouble(kv, pre + "homing_current_ma", d.run_current_ma);
|
|
d.travel_deg = getDouble(kv, pre + "travel_deg", d.travel_deg);
|
|
d.scan_speed_deg_s = getDouble(kv, pre + "scan_speed_deg_s", d.scan_speed_deg_s);
|
|
d.homing_speed_deg_s = getDouble(kv, pre + "homing_speed_deg_s", d.homing_speed_deg_s);
|
|
d.center_speed_deg_s = getDouble(kv, pre + "center_speed_deg_s", d.center_speed_deg_s);
|
|
d.steps_per_rev = getInt(kv, pre + "steps_per_rev", static_cast<int>(d.steps_per_rev));
|
|
d.enc_deviation = getInt(kv, pre + "enc_deviation", static_cast<int>(d.enc_deviation));
|
|
d.enc_const = getDouble(kv, pre + "enc_const", d.enc_const);
|
|
d.wiring_declared = kv.count(pre + "sw_mode") || kv.count(pre + "has_encoder");
|
|
d.sw_mode = static_cast<unsigned>(getInt(kv, pre + "sw_mode", static_cast<int>(d.sw_mode)));
|
|
d.has_encoder = getBool(kv, pre + "has_encoder", d.has_encoder);
|
|
};
|
|
axisDrivetrain("yaw", cfg.drivetrain.yaw);
|
|
axisDrivetrain("pitch", cfg.drivetrain.pitch);
|
|
|
|
// [Scan]: the scan-grid CSV. No file, no auto-sweep - nothing is generated.
|
|
cfg.scan.grid_file = get(kv, "Scan.grid_file", cfg.scan.grid_file);
|
|
|
|
// ---- validation ----
|
|
if (!(cfg.general.image_interval > 0) || !std::isfinite(cfg.general.image_interval))
|
|
throw std::runtime_error("General.image_interval must be > 0 (got " +
|
|
get(kv, "General.image_interval") + ")");
|
|
if (cfg.serial.baud == 0)
|
|
throw std::runtime_error("Serial.baud must be > 0");
|
|
|
|
// A zero scale makes every degree map to the centre and every count read back
|
|
// as the centre - the axis would look calibrated and point nowhere.
|
|
if (std::abs(cfg.geometry.yaw.counts_per_deg) < 1e-9)
|
|
throw std::runtime_error("Motor.yaw_counts_per_deg must be non-zero");
|
|
if (std::abs(cfg.geometry.pitch.counts_per_deg) < 1e-9)
|
|
throw std::runtime_error("Motor.pitch_counts_per_deg must be non-zero");
|
|
// The centre count exists so the whole working range stays positive; a
|
|
// non-positive one defeats the point and can drive the firmware negative.
|
|
if (cfg.geometry.yaw.center_count <= 0)
|
|
throw std::runtime_error("Motor.yaw_center_count must be > 0");
|
|
if (cfg.geometry.pitch.center_count <= 0)
|
|
throw std::runtime_error("Motor.pitch_center_count must be > 0");
|
|
if (cfg.yaw_soft_margin_counts < 0)
|
|
throw std::runtime_error("Motor.yaw_soft_margin_counts must be >= 0");
|
|
if (cfg.pitch_soft_margin_counts < 0)
|
|
throw std::runtime_error("Motor.pitch_soft_margin_counts must be >= 0");
|
|
if (cfg.homing_timeout_s <= 0)
|
|
throw std::runtime_error("Motor.homing_timeout_s must be > 0");
|
|
|
|
// [Motor.<axis>]. Currents are only meaningful alongside the sense resistor
|
|
// that scales them, and a current without one would be computed against a
|
|
// guess - so demand them together rather than inventing a default resistor.
|
|
auto checkDrivetrain = [](const char* axis, const AxisDrivetrainConfig& d) {
|
|
const std::string sec = std::string("Motor.") + axis;
|
|
if (d.run_current_ma > 0.0 && d.r_sense_ohm <= 0.0)
|
|
throw std::runtime_error(sec + ".run_current_ma needs " + sec +
|
|
".r_sense_ohm (the sense resistor on that driver "
|
|
"board) to mean anything");
|
|
if (d.run_current_ma < 0.0 || d.homing_current_ma < 0.0)
|
|
throw std::runtime_error(sec + " currents must be >= 0");
|
|
if (d.travel_deg < 0.0 || d.travel_deg > 720.0)
|
|
throw std::runtime_error(sec + ".travel_deg must be within (0, 720]; the "
|
|
"direction of travel is carried by the sign of "
|
|
"counts_per_deg, not by this angle");
|
|
if (d.enc_const != 0.0 && !encConstRegister(d.enc_const))
|
|
throw std::runtime_error(sec + ".enc_const must be between 0 and 32768 with at "
|
|
"most 4 decimals (microsteps per encoder count; "
|
|
"leave it out for the default 12.8)");
|
|
};
|
|
checkDrivetrain("yaw", cfg.drivetrain.yaw);
|
|
checkDrivetrain("pitch", cfg.drivetrain.pitch);
|
|
|
|
// Keys from the pre-centre-anchoring layout. Silently ignoring them would
|
|
// leave an operator convinced they had configured a heading that is in fact
|
|
// coming from somewhere else entirely - the exact failure this layout removes.
|
|
static const char* kRemovedMotorKeys[] = {
|
|
"Motor.yaw_zero_count", "Motor.pitch_zero_count", "Motor.yaw_min_deg",
|
|
"Motor.yaw_max_deg", "Motor.pitch_min_deg", "Motor.pitch_max_deg",
|
|
"Motor.yaw_home_zero",
|
|
// Drivetrain keys written under [Motor] instead of [Motor.<axis>]. They
|
|
// flatten to a name nothing reads, so without this they would be ignored
|
|
// in total silence - and an operator would believe they had reduced the
|
|
// homing current when they had not.
|
|
"Motor.r_sense_ohm", "Motor.run_current_ma",
|
|
"Motor.homing_current_ma", "Motor.travel_deg",
|
|
};
|
|
std::string stale;
|
|
for (const char* k : kRemovedMotorKeys)
|
|
if (kv.count(k)) stale += (stale.empty() ? "" : ", ") + std::string(k + 6);
|
|
if (!stale.empty())
|
|
LOG_WARN << "[Motor] ignoring keys nothing reads: " << stale
|
|
<< " -- travel limits are derived from homing, the heading origin "
|
|
"comes from yaw_heading_at_center (set it with 'gimbal heading "
|
|
"setup'), and drivetrain keys belong under [Motor.yaw] / "
|
|
"[Motor.pitch], not [Motor]";
|
|
|
|
// Keys from when a blank grid_file meant "generate a grid". The generator is
|
|
// gone: the CSV is the only source. Ignoring these quietly would leave an
|
|
// operator believing they had set the scan arc when nothing reads it.
|
|
static const char* kRemovedScanKeys[] = {
|
|
"Scan.yaw_intervals", "Scan.yaw_min_deg", "Scan.yaw_max_deg", "Scan.pitch_levels",
|
|
};
|
|
std::string stale_scan;
|
|
for (const char* k : kRemovedScanKeys)
|
|
if (kv.count(k)) stale_scan += (stale_scan.empty() ? "" : ", ") + std::string(k + 5);
|
|
if (!stale_scan.empty())
|
|
LOG_WARN << "[Scan] ignoring keys nothing reads: " << stale_scan
|
|
<< " -- grids are no longer generated; list the waypoints in the "
|
|
"grid_file CSV instead";
|
|
|
|
return cfg;
|
|
}
|
|
|
|
SensorEnable resolveSensorEnable(const std::string& declared, bool flag_value, bool flag_present) {
|
|
SensorEnable r;
|
|
if (declared.empty()) {
|
|
// Nothing stated, so the legacy flag still governs. This is every tower
|
|
// that has not been migrated, and it must behave exactly as before.
|
|
r.enabled = flag_value;
|
|
r.source = flag_present ? SensorEnableSource::Flag : SensorEnableSource::Default;
|
|
return r;
|
|
}
|
|
r.enabled = (declared != "none");
|
|
r.source = SensorEnableSource::Declaration;
|
|
r.conflict = flag_present && (flag_value != r.enabled);
|
|
return r;
|
|
}
|
|
|
|
bool HardwareConfig::hasAxis(const std::string& name) const {
|
|
// An undeclared rig answers true for everything - see the header.
|
|
if (axes.empty()) return true;
|
|
return std::find(axes.begin(), axes.end(), name) != axes.end();
|
|
}
|
|
|
|
std::string HardwareConfig::axesLabel() const {
|
|
std::string s;
|
|
for (const auto& a : axes) s += (s.empty() ? "" : ", ") + a;
|
|
return s;
|
|
}
|
|
|
|
AppConfig ConfigLoader::loadFromFile(const std::string& path, const std::string& hardware_path) {
|
|
std::map<std::string, std::string> kv;
|
|
int rc = ini_parse(path.c_str(), iniHandler, &kv);
|
|
if (rc < 0) throw std::runtime_error("Cannot read config file: " + path);
|
|
if (rc > 0)
|
|
throw std::runtime_error("Parse error in config file " + path + " at line " +
|
|
std::to_string(rc));
|
|
|
|
// hardware.ini is layered ON TOP, so the assembly description wins over a
|
|
// config.ini that still carries the same keys from before the split. Any such
|
|
// leftover is named once, because it is a migration leftover to delete rather
|
|
// than a setting that is quietly doing nothing.
|
|
std::vector<std::string> shadowed;
|
|
if (!hardware_path.empty()) {
|
|
std::map<std::string, std::string> hw;
|
|
int hrc = ini_parse(hardware_path.c_str(), iniHandler, &hw);
|
|
if (hrc < 0) throw std::runtime_error("Cannot read hardware file: " + hardware_path);
|
|
if (hrc > 0)
|
|
throw std::runtime_error("Parse error in hardware file " + hardware_path +
|
|
" at line " + std::to_string(hrc));
|
|
for (const auto& [k, v] : hw) {
|
|
if (auto it = kv.find(k); it != kv.end() && it->second != v) shadowed.push_back(k);
|
|
kv[k] = v;
|
|
}
|
|
}
|
|
|
|
AppConfig cfg = fromMap(kv);
|
|
|
|
if (!hardware_path.empty()) {
|
|
cfg.hardware.declared = true;
|
|
cfg.hardware.source_path = hardware_path;
|
|
LOG_INFO << "hardware: " << hardware_path
|
|
<< (cfg.board.model.empty() ? "" : " board " + cfg.board.model)
|
|
<< (cfg.hardware.axes.empty() ? "" : " axes " + cfg.hardware.axesLabel());
|
|
for (const auto& k : shadowed)
|
|
LOG_WARN << "config.ini still sets " << k
|
|
<< ", which hardware.ini overrides -- delete it from config.ini";
|
|
}
|
|
// Anchor a relative scan grid_file to the config file's directory so it resolves
|
|
// no matter which working directory the process is launched from (the config
|
|
// itself is found via a search path that may be unrelated to the cwd).
|
|
if (!cfg.scan.grid_file.empty()) {
|
|
std::filesystem::path gf(paths::expandUser(cfg.scan.grid_file));
|
|
if (gf.is_relative()) gf = std::filesystem::path(path).parent_path() / gf;
|
|
cfg.scan.grid_file = gf.lexically_normal().string();
|
|
}
|
|
// Same for each camera's profile directory.
|
|
for (auto& c : cfg.camera.cameras) {
|
|
std::filesystem::path pd(paths::expandUser(c.profile_dir));
|
|
if (pd.is_relative()) pd = std::filesystem::path(path).parent_path() / pd;
|
|
c.profile_dir = pd.lexically_normal().string();
|
|
}
|
|
return cfg;
|
|
}
|
|
|
|
namespace {
|
|
std::string trimmed(const std::string& s) {
|
|
const char* ws = " \t\r\n";
|
|
size_t a = s.find_first_not_of(ws);
|
|
if (a == std::string::npos) return "";
|
|
size_t b = s.find_last_not_of(ws);
|
|
return s.substr(a, b - a + 1);
|
|
}
|
|
|
|
// Marker for a superseded value, kept so a calibration can be reverted by hand.
|
|
// It is a comment, so inih ignores it on the next read, but it is distinctive
|
|
// enough for updateIniSectionKeys to find its own earlier markers and replace
|
|
// them rather than pile them up.
|
|
const std::string kPrevMarker = ";prev ";
|
|
|
|
// The value inih would actually parse from the right-hand side of `key = value`:
|
|
// everything up to an inline comment, which is a ';' PRECEDED BY WHITESPACE (see
|
|
// ini_find_chars_or_comment in ini.c, with INI_ALLOW_INLINE_COMMENTS=1 and
|
|
// INI_INLINE_COMMENT_PREFIXES=";"). Mirroring that rule matters here: "did this
|
|
// value change?" has to compare what the program read, not the raw line, or a
|
|
// key carrying a trailing comment would look changed on every save.
|
|
// Do these two INI values denote the same NUMBER, however they are spelled?
|
|
//
|
|
// The history filter below compares what is in the file against what is about to
|
|
// be written, and the writer formats doubles with "%.10g" - so a hand-written
|
|
// "0.0" is rewritten as "0", and a plain string comparison calls that a change.
|
|
// It then records a ";prev" line whose "previous" value is the value it still
|
|
// has, stamped with the provenance of whatever save happened to be running.
|
|
//
|
|
// That is worse than cosmetic noise: ";prev" deliberately keeps exactly ONE
|
|
// generation, so a spurious entry DISPLACES the real previous value it exists to
|
|
// preserve, and attributes an untouched key to a calibration that never wrote it.
|
|
//
|
|
// Falls back to comparing text whenever either side is not a plain number, which
|
|
// is what keys like a device path need.
|
|
bool sameIniValue(const std::string& a, const std::string& b) {
|
|
if (a == b) return true;
|
|
auto asNumber = [](const std::string& s, double& out) {
|
|
if (s.empty()) return false;
|
|
char* end = nullptr;
|
|
out = std::strtod(s.c_str(), &end);
|
|
if (end == s.c_str()) return false; // nothing numeric at all
|
|
while (*end && std::isspace(static_cast<unsigned char>(*end))) ++end;
|
|
return *end == '\0'; // trailing junk: not a number
|
|
};
|
|
double x = 0.0, y = 0.0;
|
|
return asNumber(a, x) && asNumber(b, y) && x == y;
|
|
}
|
|
|
|
std::string iniValueOf(const std::string& rhs) {
|
|
bool was_space = false;
|
|
for (size_t i = 0; i < rhs.size(); ++i) {
|
|
if (was_space && rhs[i] == ';') return trimmed(rhs.substr(0, i));
|
|
was_space = std::isspace(static_cast<unsigned char>(rhs[i])) != 0;
|
|
}
|
|
return trimmed(rhs);
|
|
}
|
|
|
|
// If `t` (already trimmed) is a ";prev <key> = ..." marker, return <key>, else "".
|
|
std::string prevMarkerKey(const std::string& t) {
|
|
const size_t n = kPrevMarker.size();
|
|
if (t.compare(0, n, kPrevMarker) != 0) return "";
|
|
const size_t eq = t.find('=', n);
|
|
if (eq == std::string::npos) return "";
|
|
return trimmed(t.substr(n, eq - n));
|
|
}
|
|
} // namespace
|
|
|
|
std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
|
|
const std::vector<std::pair<std::string, std::string>>& kv,
|
|
const std::string& provenance) {
|
|
// Which keys are actually changing, and what they hold now.
|
|
//
|
|
// Only a changing key gets a ";prev" line, and only its marker is replaced:
|
|
// a key whose value is unchanged keeps the history it has, because that still
|
|
// names the value before the current one. This also keeps the file from
|
|
// growing on every save - saveMotorGeometry rewrites all eight keys each time,
|
|
// and without this filter seven of them would be noise.
|
|
//
|
|
// Computed in a first pass because a key's ";prev" marker sits ABOVE its value
|
|
// line, so the writing pass meets the marker before it has seen whether the
|
|
// value differs.
|
|
std::vector<bool> changed(kv.size(), false);
|
|
std::vector<std::string> old_value(kv.size());
|
|
{
|
|
std::istringstream scan(contents);
|
|
std::string line, cur;
|
|
std::vector<bool> seen(kv.size(), false);
|
|
while (std::getline(scan, line)) {
|
|
const std::string t = trimmed(line);
|
|
if (t.size() >= 2 && t.front() == '[' && t.back() == ']') {
|
|
cur = trimmed(t.substr(1, t.size() - 2));
|
|
continue;
|
|
}
|
|
if (cur != section || t.empty() || t[0] == ';' || t[0] == '#') continue;
|
|
const size_t eq = line.find('=');
|
|
if (eq == std::string::npos) continue;
|
|
const std::string key = trimmed(line.substr(0, eq));
|
|
for (size_t i = 0; i < kv.size(); ++i) {
|
|
if (!seen[i] && key == kv[i].first) {
|
|
seen[i] = true;
|
|
old_value[i] = iniValueOf(line.substr(eq + 1));
|
|
changed[i] = !sameIniValue(old_value[i], kv[i].second);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
const bool keep_history = !provenance.empty();
|
|
|
|
std::vector<bool> written(kv.size(), false);
|
|
std::istringstream in(contents);
|
|
std::ostringstream out;
|
|
std::string line, cur;
|
|
while (std::getline(in, line)) {
|
|
const std::string t = trimmed(line);
|
|
if (t.size() >= 2 && t.front() == '[' && t.back() == ']') {
|
|
cur = trimmed(t.substr(1, t.size() - 2));
|
|
} else if (cur == section) {
|
|
// An earlier ";prev" for a key that is changing now is dropped here
|
|
// and re-emitted below, so exactly one generation is ever kept.
|
|
const std::string pk = prevMarkerKey(t);
|
|
if (!pk.empty()) {
|
|
bool supersede = false;
|
|
for (size_t i = 0; i < kv.size(); ++i)
|
|
if (pk == kv[i].first && changed[i]) { supersede = true; break; }
|
|
if (supersede) continue;
|
|
} else if (!t.empty() && t[0] != ';' && t[0] != '#') {
|
|
const size_t eq = line.find('=');
|
|
if (eq != std::string::npos) {
|
|
const std::string key = trimmed(line.substr(0, eq));
|
|
for (size_t i = 0; i < kv.size(); ++i) {
|
|
if (!written[i] && key == kv[i].first) {
|
|
if (keep_history && changed[i])
|
|
out << kPrevMarker << kv[i].first << " = " << old_value[i]
|
|
<< " ; " << provenance << "\n";
|
|
line = kv[i].first + " = " + kv[i].second;
|
|
written[i] = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
out << line << "\n";
|
|
}
|
|
// Keys the section does not have yet go INTO it, after its last key line, with
|
|
// a note of when and why. A second [section] block at the end reads the same
|
|
// (inih merges duplicates) but is not where a person reading the file looks -
|
|
// on tower 111 it left `cooling` a hundred lines away from `env`. Only a
|
|
// section the file lacks altogether is appended.
|
|
bool missing = false;
|
|
for (bool w : written) missing = missing || !w;
|
|
if (!missing) return out.str();
|
|
|
|
std::vector<std::string> block;
|
|
if (keep_history) block.push_back("; added ; " + provenance);
|
|
for (size_t i = 0; i < kv.size(); ++i)
|
|
if (!written[i]) block.push_back(kv[i].first + " = " + kv[i].second);
|
|
|
|
std::vector<std::string> lines;
|
|
{
|
|
std::istringstream split(out.str());
|
|
for (std::string l; std::getline(split, l);) lines.push_back(l);
|
|
}
|
|
long header = -1, last_key = -1;
|
|
for (size_t i = 0; i < lines.size(); ++i) {
|
|
const std::string t = trimmed(lines[i]);
|
|
if (t.size() >= 2 && t.front() == '[' && t.back() == ']') {
|
|
if (header >= 0) break; // the end of the first block of this section
|
|
if (trimmed(t.substr(1, t.size() - 2)) == section) header = static_cast<long>(i);
|
|
continue;
|
|
}
|
|
if (header >= 0 && !t.empty() && t[0] != ';' && t[0] != '#' &&
|
|
t.find('=') != std::string::npos)
|
|
last_key = static_cast<long>(i);
|
|
}
|
|
std::ostringstream res;
|
|
if (header < 0) {
|
|
res << out.str() << "\n[" << section << "]\n";
|
|
for (const auto& b : block) res << b << "\n";
|
|
return res.str();
|
|
}
|
|
const long after = last_key >= 0 ? last_key : header;
|
|
for (size_t i = 0; i < lines.size(); ++i) {
|
|
res << lines[i] << "\n";
|
|
if (static_cast<long>(i) == after)
|
|
for (const auto& b : block) res << b << "\n";
|
|
}
|
|
return res.str();
|
|
}
|
|
|
|
bool saveMotorGeometry(const std::string& path, const Geometry& geo, long yaw_margin,
|
|
long pitch_margin, const std::string& provenance) {
|
|
if (path.empty()) return false;
|
|
std::ifstream f(path);
|
|
if (!f) return false;
|
|
std::stringstream buf;
|
|
buf << f.rdbuf();
|
|
f.close();
|
|
|
|
auto num = [](double v) {
|
|
char b[32];
|
|
std::snprintf(b, sizeof(b), "%.10g", v);
|
|
return std::string(b);
|
|
};
|
|
// The whole [Motor] block, so it stays complete and self-consistent rather
|
|
// than depending on which lines happened to be there already. min_deg/max_deg
|
|
// are deliberately absent: they are derived from homing, not configured.
|
|
const std::vector<std::pair<std::string, std::string>> kv = {
|
|
{"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
|
|
{"yaw_center_count", std::to_string(geo.yaw.center_count)},
|
|
{"yaw_heading_at_center", num(geo.yaw.deg_at_center)},
|
|
{"yaw_soft_margin_counts", std::to_string(yaw_margin)},
|
|
{"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
|
|
{"pitch_center_count", std::to_string(geo.pitch.center_count)},
|
|
{"pitch_angle_at_center", num(geo.pitch.deg_at_center)},
|
|
{"pitch_soft_margin_counts", std::to_string(pitch_margin)},
|
|
};
|
|
const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv, provenance);
|
|
|
|
// Write a sibling temp file and rename over the original: a truncating write
|
|
// that dies partway leaves config.ini destroyed, and three commands rewrite
|
|
// this file while the program is running.
|
|
std::error_code ec;
|
|
const fs::path target(path);
|
|
const fs::path tmp = target.parent_path() / (target.filename().string() + ".tmp");
|
|
{
|
|
std::ofstream out(tmp, std::ios::trunc | std::ios::binary);
|
|
if (!out) return false;
|
|
out << updated;
|
|
out.flush();
|
|
if (!out) {
|
|
fs::remove(tmp, ec);
|
|
return false;
|
|
}
|
|
}
|
|
fs::rename(tmp, target, ec);
|
|
if (ec) {
|
|
fs::remove(tmp, ec);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} // namespace fgc
|