273 lines
12 KiB
C++
273 lines
12 KiB
C++
#pragma once
|
|
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <map>
|
|
#include <mutex>
|
|
#include <set>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
// Simulated motor controller for development without hardware. Models the new
|
|
// firmware's encoder-count protocol: HOME makes both axes READY; MOVE sets a
|
|
// per-axis target; each telemetry() read steps the encoder toward its target
|
|
// and reports standstill once reached - so the capture scheduler's
|
|
// move -> settle -> trigger loop runs exactly as it would against real hardware.
|
|
class MockMotorController : public IMotorController {
|
|
public:
|
|
// `needcfg`: emit NEED lines on start and refuse to move until they are
|
|
// answered, the way the real firmware will (hardware-config-plan.md
|
|
// section 6). Off by default so every existing test sees the old mock.
|
|
//
|
|
// This is what lets the HOST half of the handshake be built and verified
|
|
// before the firmware half exists - and it is not throwaway: it stays as the
|
|
// way the handshake is exercised on a dev box with no rig.
|
|
explicit MockMotorController(bool needcfg = false) : needcfg_(needcfg) {}
|
|
|
|
void start() override {
|
|
LOG_INFO << "[mock] motor controller started";
|
|
if (!needcfg_) return;
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
static const char* kKeys[] = {"steps_per_rev", "global_scaler", "run_irun",
|
|
"homing_irun", "sw_mode", "enc_deviation",
|
|
"normal_speed", "homing_speed", "center_speed",
|
|
"center", "margin"};
|
|
need_lines_.clear();
|
|
unanswered_.clear();
|
|
for (const char* axis : {"Y", "P"}) {
|
|
std::string line = std::string("NEED ") + axis;
|
|
for (const char* k : kKeys) {
|
|
line += std::string(" ") + k;
|
|
// Track the same set the line asks for, so the axis unlocks only
|
|
// when EVERY key has been answered - a partial answer must leave
|
|
// it in NEEDCFG, which is the behaviour worth simulating.
|
|
unanswered_.insert(std::string(axis) + "." + k);
|
|
}
|
|
need_lines_.push_back(line);
|
|
}
|
|
configured_ = false;
|
|
LOG_INFO << "[mock] NEEDCFG: asking the host for config, refusing to move until answered";
|
|
}
|
|
void stop() override { LOG_INFO << "[mock] motor controller stopped"; }
|
|
|
|
void sendCommand(const std::string& cmd) override {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
std::istringstream in(cmd);
|
|
std::string verb;
|
|
in >> verb;
|
|
for (auto& c : verb) c = static_cast<char>(std::toupper(static_cast<unsigned char>(c)));
|
|
|
|
if (verb == "SET") {
|
|
// Record what the host answered, so a test can assert the handshake
|
|
// actually put the right values on the wire.
|
|
std::string axis, key, value;
|
|
in >> axis >> key >> value;
|
|
settings_[axis + "." + key] = value;
|
|
unanswered_.erase(axis + "." + key);
|
|
if (unanswered_.empty()) configured_ = true;
|
|
return;
|
|
}
|
|
if (needcfg_ && !configured_ &&
|
|
(verb == "HOME" || verb == "MOVE" || verb == "ENABLE")) {
|
|
LOG_WARN << "[mock] ERR need config (" << verb << " rejected)";
|
|
return;
|
|
}
|
|
if (verb == "HOME") {
|
|
homed_ = true;
|
|
deviation_ = false; // homing clears the latch, as on the real driver
|
|
} else if (verb == "STALL") {
|
|
// Test hook: fake the hardware encoder-deviation latch so the STALL
|
|
// badge is reachable without jamming a real axis. Latches like the
|
|
// real one - only HOME/RESET clears it.
|
|
deviation_ = true;
|
|
} else if (verb == "RESET") {
|
|
deviation_ = false;
|
|
// A RESET is not a way out of NEEDCFG (firmware motor.cpp says why).
|
|
if (needcfg_ && !configured_) homed_ = false;
|
|
} else if (verb == "MOVE") {
|
|
std::string a;
|
|
in >> a;
|
|
auto comma = a.find(',');
|
|
if (comma != std::string::npos) { // MOVE <yaw>,<pitch>
|
|
tryParse(a.substr(0, comma), yaw_target_);
|
|
tryParse(a.substr(comma + 1), pitch_target_);
|
|
} else { // MOVE Y|P <pos>
|
|
long pos = 0;
|
|
if (in >> pos) {
|
|
if (a == "Y" || a == "y") yaw_target_ = pos;
|
|
else if (a == "P" || a == "p") pitch_target_ = pos;
|
|
}
|
|
}
|
|
} else if (verb == "STOP") {
|
|
yaw_target_ = yaw_.xenc;
|
|
pitch_target_ = pitch_.xenc;
|
|
} else if (verb == "DUMP") {
|
|
// Mirror the real firmware DUMP wire format (see firmware/src/motor.cpp
|
|
// and main.cpp) so the host-side DumpParser is exercised without
|
|
// hardware. Register values are illustrative, not simulated; the
|
|
// encoder positions track the mock's live state.
|
|
// 5 = NEEDCFG, appended to the firmware enum so the older codes keep
|
|
// their meanings. See motor.h.
|
|
const int st = (needcfg_ && !configured_) ? 5 : (homed_ ? 3 : 1);
|
|
std::ostringstream d;
|
|
d << "DUMP BEGIN build=mock uptime=0 mcusr=0x01 free_ram=4096\n";
|
|
auto axis = [&](char L, const AxisTelemetry& a) {
|
|
// In NEEDCFG mode, echo back what the host actually SET, so the
|
|
// whole round trip - NEED, SET, DUMP, `test hardware motor` -
|
|
// can be exercised with no firmware. Outside it, stay on the old
|
|
// fixed line: that is a firmware predating the handshake, and
|
|
// the host must keep telling the two apart.
|
|
std::string owned = " speed=200000 center=500000 soft_margin=5000";
|
|
if (needcfg_) {
|
|
const std::string ax(1, L);
|
|
auto val = [&](const char* k, const char* dflt) {
|
|
auto it = settings_.find(ax + "." + k);
|
|
return it == settings_.end() ? std::string(dflt) : it->second;
|
|
};
|
|
owned = " speed=" + val("normal_speed", "200000") +
|
|
" center=" + val("center", "500000") +
|
|
" soft_margin=" + val("margin", "5000") +
|
|
" steps_per_rev=" + val("steps_per_rev", "0") +
|
|
" enc_dev=" + val("enc_deviation", "0") +
|
|
" homing_speed=" + val("homing_speed", "0") +
|
|
" center_speed=" + val("center_speed", "0") +
|
|
" sw_mode=" + val("sw_mode", "0") +
|
|
" gs=" + val("global_scaler", "0") +
|
|
" irun=" + val("run_irun", "0") +
|
|
" irun_home=" + val("homing_irun", "0");
|
|
}
|
|
d << "DUMP " << L << " state=" << st
|
|
<< " hsub=0 enabled=" << (homed_ ? 1 : 0)
|
|
<< " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
|
|
<< owned
|
|
<< " hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1\n";
|
|
d << "DUMP " << L << " TMC GCONF=0x00000004 GSTAT=0x00000000"
|
|
<< " IOIN=0x30000008 TSTEP=0x000fffff RAMPMODE=0x00000000"
|
|
<< " XACTUAL=0x" << std::hex << (a.xactual & 0xffffffff)
|
|
<< " VACTUAL=0x00000000 XTARGET=0x" << (a.xenc & 0xffffffff) << std::dec
|
|
<< " SW_MODE=0x00000000 RAMP_STAT=0x00000300 X_ENC=0x" << std::hex
|
|
<< (a.xenc & 0xffffffff) << std::dec
|
|
<< " ENC_STATUS=0x00000000 CHOPCONF=0x10410150"
|
|
<< " DRV_STATUS=0x80084000 PWM_SCALE=0x00000008 PWM_AUTO=0x00000000\n";
|
|
};
|
|
axis('Y', yaw_);
|
|
axis('P', pitch_);
|
|
d << "DUMP END\n";
|
|
dump_ = d.str();
|
|
LOG_INFO << "firmware dump:\n" << dump_;
|
|
} else if (verb == "DIAG") {
|
|
// Canned DG stream (real firmware format) so the host DiagParser /
|
|
// logfile path are demoable without hardware. Logged live like the
|
|
// real capture would be.
|
|
std::ostringstream d;
|
|
auto axis = [&](char L) {
|
|
d << "DG BEGIN " << L << "\n";
|
|
const int speeds[3] = {12500, 25000, 50000};
|
|
for (int s : speeds)
|
|
for (bool fwd : {true, false})
|
|
d << "DG " << L << " S" << s << (fwd ? " FWD" : " REV")
|
|
<< " ERR_PEAK 120 ERR_RMS 30 ERR_STILL 40 CS_MIN 10 CS_MAX 20"
|
|
<< " SG_MIN 300 PWM_AVG 96 FLAGS 0x00000000 PASS\n";
|
|
d << "DG " << L << " RESULT PASS\n";
|
|
};
|
|
axis('Y');
|
|
axis('P');
|
|
d << "DG DONE\n";
|
|
diag_ = d.str();
|
|
++diag_seq_;
|
|
std::istringstream ds(diag_);
|
|
std::string ln;
|
|
while (std::getline(ds, ln)) LOG_INFO << ln; // stream to LOG pane
|
|
}
|
|
// ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
|
|
}
|
|
|
|
MotorTelemetry telemetry() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
step(yaw_, yaw_target_);
|
|
step(pitch_, pitch_target_);
|
|
MotorTelemetry t;
|
|
t.yaw = yaw_;
|
|
t.pitch = pitch_;
|
|
t.pitch_present = true;
|
|
return t;
|
|
}
|
|
|
|
std::string lastDump() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return dump_;
|
|
}
|
|
|
|
std::string lastDiag() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return diag_;
|
|
}
|
|
unsigned diagSeq() const override { return diag_seq_; }
|
|
|
|
std::vector<std::string> takeNeedLines() override {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
std::vector<std::string> out;
|
|
out.swap(need_lines_);
|
|
return out;
|
|
}
|
|
|
|
// Test/inspection hooks: what the host SET, and whether the axis is unlocked.
|
|
std::map<std::string, std::string> settings() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return settings_;
|
|
}
|
|
bool configured() const { return configured_; }
|
|
|
|
bool connected() const override { return true; }
|
|
|
|
private:
|
|
bool needcfg_ = false;
|
|
bool configured_ = false;
|
|
std::vector<std::string> need_lines_;
|
|
std::map<std::string, std::string> settings_;
|
|
std::set<std::string> unanswered_;
|
|
|
|
static constexpr long kStep = 80000; // counts advanced per telemetry read
|
|
|
|
void step(AxisTelemetry& a, long target) {
|
|
long d = target - a.xenc;
|
|
if (d >= -kStep && d <= kStep) {
|
|
a.xenc = target;
|
|
a.standstill = true;
|
|
} else {
|
|
a.xenc += (d > 0 ? kStep : -kStep);
|
|
a.standstill = false;
|
|
}
|
|
a.xactual = a.xenc;
|
|
// An axis waiting on its config says so, rather than looking like a
|
|
// healthy un-homed one - that distinction is the whole point of the state.
|
|
a.state = (needcfg_ && !configured_) ? AxisState::NeedConfig
|
|
: (homed_ ? AxisState::Ready : AxisState::Reset);
|
|
a.deviation = deviation_;
|
|
}
|
|
|
|
static void tryParse(const std::string& s, long& out) {
|
|
try {
|
|
out = std::stol(s);
|
|
} catch (const std::exception&) {}
|
|
}
|
|
|
|
mutable std::mutex mutex_;
|
|
AxisTelemetry yaw_;
|
|
AxisTelemetry pitch_;
|
|
long yaw_target_ = 0;
|
|
long pitch_target_ = 0;
|
|
bool homed_ = false;
|
|
bool deviation_ = false; // faked encoder-deviation latch (see STALL above)
|
|
std::string dump_;
|
|
std::string diag_;
|
|
unsigned diag_seq_ = 0;
|
|
};
|
|
|
|
} // namespace fgc
|