130 lines
4.8 KiB
C++
130 lines
4.8 KiB
C++
#pragma once
|
|
|
|
#include "fgc/IMotorController.h"
|
|
#include "fgc/Logger.h"
|
|
|
|
#include <mutex>
|
|
#include <sstream>
|
|
#include <string>
|
|
|
|
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:
|
|
void start() override { LOG_INFO << "[mock] motor controller started"; }
|
|
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 == "HOME") {
|
|
homed_ = true;
|
|
} 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.
|
|
const int st = homed_ ? 3 : 1; // READY : RESET
|
|
std::ostringstream d;
|
|
d << "DUMP BEGIN build=mock uptime=0 mcusr=0x01 free_ram=4096\n";
|
|
auto axis = [&](char L, const AxisTelemetry& a) {
|
|
d << "DUMP " << L << " state=" << st
|
|
<< " hsub=0 enabled=" << (homed_ ? 1 : 0)
|
|
<< " lim_neg=0 lim_pos=1000000 hold_target=" << a.xenc
|
|
<< " speed=200000 eeprom_restored=1 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_;
|
|
}
|
|
// 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_;
|
|
}
|
|
|
|
bool connected() const override { return true; }
|
|
|
|
private:
|
|
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;
|
|
a.state = homed_ ? AxisState::Ready : AxisState::Reset;
|
|
}
|
|
|
|
static void tryParse(const std::string& s, long& out) {
|
|
try {
|
|
out = std::stol(s);
|
|
} catch (const std::exception&) {}
|
|
}
|
|
|
|
std::mutex mutex_;
|
|
AxisTelemetry yaw_;
|
|
AxisTelemetry pitch_;
|
|
long yaw_target_ = 0;
|
|
long pitch_target_ = 0;
|
|
bool homed_ = false;
|
|
std::string dump_;
|
|
};
|
|
|
|
} // namespace fgc
|