114 lines
4.2 KiB
C++
114 lines
4.2 KiB
C++
#include "fgc/FirmwareConfig.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
|
|
namespace fgc {
|
|
namespace {
|
|
|
|
// One answerable key: where its value comes from, and whether the host has it.
|
|
struct Answer {
|
|
bool have = false;
|
|
long long value = 0;
|
|
};
|
|
|
|
Answer answerFor(const std::string& key, const AxisConfigSource& s) {
|
|
// Motor current. All three come from the same conversion, so they are
|
|
// available together or not at all.
|
|
if (key == "global_scaler") return {s.have_current, s.global_scaler};
|
|
if (key == "run_irun") return {s.have_current, s.run_irun};
|
|
if (key == "homing_irun") return {s.have_current, s.homing_irun};
|
|
|
|
// Position anchors. `center` / `margin` are the names the existing CENTER
|
|
// and MARGIN commands use.
|
|
if (key == "center_count" || key == "center") return {true, s.center_count};
|
|
if (key == "soft_margin" || key == "margin") return {true, s.soft_margin};
|
|
|
|
if (key == "steps_per_rev") return {s.have_travel, s.steps_per_rev};
|
|
if (key == "enc_deviation") return {s.have_deviation, s.enc_deviation};
|
|
|
|
// A VMAX of 0 is not a slow axis, it is a stopped one, so an unconverted
|
|
// speed is reported as missing rather than sent.
|
|
if (key == "normal_speed") return {s.normal_vmax > 0, s.normal_vmax};
|
|
if (key == "homing_speed") return {s.homing_vmax > 0, s.homing_vmax};
|
|
if (key == "center_speed") return {s.center_vmax > 0, s.center_vmax};
|
|
|
|
if (key == "sw_mode") return {s.have_wiring, static_cast<long long>(s.sw_mode)};
|
|
if (key == "has_encoder") return {s.have_wiring, s.has_encoder ? 1 : 0};
|
|
if (key == "enc_const") return {s.have_enc_const, s.enc_const};
|
|
|
|
return {}; // not in the vocabulary
|
|
}
|
|
|
|
bool known(const std::string& key) {
|
|
static const char* kKeys[] = {
|
|
"global_scaler", "run_irun", "homing_irun", "center_count", "center",
|
|
"soft_margin", "margin", "steps_per_rev", "enc_deviation",
|
|
"normal_speed", "homing_speed", "center_speed", "sw_mode", "has_encoder",
|
|
"enc_const",
|
|
};
|
|
for (const char* k : kKeys)
|
|
if (key == k) return true;
|
|
return false;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::optional<NeedRequest> parseNeedLine(const std::string& line) {
|
|
std::istringstream is(line);
|
|
std::string tok;
|
|
if (!(is >> tok) || tok != "NEED") return std::nullopt;
|
|
|
|
std::string axis;
|
|
if (!(is >> axis)) return std::nullopt;
|
|
if (axis.size() != 1) return std::nullopt;
|
|
const char a = static_cast<char>(std::toupper(static_cast<unsigned char>(axis[0])));
|
|
if (a != 'Y' && a != 'P') return std::nullopt;
|
|
|
|
NeedRequest r;
|
|
r.axis = a;
|
|
while (is >> tok) r.keys.push_back(tok);
|
|
return r;
|
|
}
|
|
|
|
FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src) {
|
|
FirmwareConfigPlan plan;
|
|
const std::string axis(1, need.axis);
|
|
|
|
// The encoder scale goes first, asked for or not: it is optional, so no NEED
|
|
// names it, and the firmware writes it to the driver with the rest of the
|
|
// set only if it has arrived by the time the last required key does.
|
|
std::vector<std::string> keys = need.keys;
|
|
if (src.have_enc_const && std::find(keys.begin(), keys.end(), "enc_const") == keys.end())
|
|
keys.insert(keys.begin(), "enc_const");
|
|
|
|
for (const auto& key : keys) {
|
|
if (!known(key)) {
|
|
// The firmware wants something this host has never heard of, which
|
|
// means it is newer. Naming it is the whole diagnostic.
|
|
plan.unknown.push_back(key);
|
|
continue;
|
|
}
|
|
const Answer a = answerFor(key, src);
|
|
if (!a.have) {
|
|
plan.unanswered.push_back(key);
|
|
continue;
|
|
}
|
|
plan.sets.push_back("SET " + axis + " " + key + " " + std::to_string(a.value));
|
|
}
|
|
return plan;
|
|
}
|
|
|
|
std::optional<long> encConstRegister(double v) {
|
|
if (!(v > 0.0) || v >= 32768.0) return std::nullopt;
|
|
const double scaled = v * 10000.0;
|
|
const double rounded = std::round(scaled);
|
|
if (std::fabs(scaled - rounded) > 1e-6) return std::nullopt;
|
|
const long whole = static_cast<long>(rounded) / 10000;
|
|
const long frac = static_cast<long>(rounded) % 10000;
|
|
return (whole << 16) | frac;
|
|
}
|
|
|
|
} // namespace fgc
|