This commit is contained in:
sagiferi
2026-05-20 21:48:13 +02:00
parent a907d8ba74
commit d3c13f997e
446 changed files with 70716 additions and 0 deletions
+184
View File
@@ -0,0 +1,184 @@
// -----------------------------------------------------------------------------------
// axis MKS SERVO42D/57D motor driver via CAN, using 0xFE absolute position updates
// EXPERIMENTAL!!!
#pragma once
#include "../../../../Common.h"
#ifdef MKS42D_MOTOR_PRESENT
#if !defined(CAN_PLUS) || CAN_PLUS == OFF
#error "No MKS motor CAN interface!"
#endif
#include "../Motor.h"
#ifndef MKS_SLEW_DIRECT
#define MKS_SLEW_DIRECT OFF
#endif
#ifndef MKS_FE_ACCEL_CONST
#define MKS_FE_ACCEL_CONST 10
#endif
// always-on status request (0xF1) rate
#ifndef MKS_STATUS_MS
#define MKS_STATUS_MS 250
#endif
// always-on protect-state request (0x3E) rate
#ifndef MKS_PROTECT_MS
#define MKS_PROTECT_MS 1000
#endif
// grace period after enabling before enforcing heartbeat
#ifndef MKS_HEARTBEAT_GRACE_MS
#define MKS_HEARTBEAT_GRACE_MS 1500
#endif
typedef struct MksDriverSettings {
int16_t model;
int8_t status; // ON/OFF (enables always-on heartbeat + protect polling)
} MksDriverSettings;
class Mks42DMotor : public Motor {
public:
Mks42DMotor(uint8_t axisNumber,
int8_t reverse,
const MksDriverSettings *Settings,
bool useFastHardwareTimers = true);
bool init() override;
uint8_t getParameterCount() override { return Motor::getParameterCount() + numParameters; }
AxisParameter* getParameter(uint8_t number) override {
const uint8_t base = Motor::getParameterCount();
if (number >= 1 && number <= base) return Motor::getParameter(number);
const uint8_t i = number - base;
if (i == 1) return &stepsPerSecondToRpm;
if (i == 2) return &maxRpm;
return &invalid;
}
// Integrated-driver style reverse control (kept for parity with other integrated CAN motors)
void setReverse(int8_t state);
// Base-class reverse (prevents silent bypass if someone calls Motor::setReverse(bool))
void setReverse(bool state) override;
void enable(bool state) override;
void resetPositionSteps(long value) override;
int getStepsPerStepSlewing() override { return 256; }
float getFrequencySteps() override;
void setFrequencySteps(float frequency) override;
void setSlewing(bool state) override;
void poll() override;
const char* name() override { return "MKS_FE"; }
// ISR uses this; not a Motor virtual
void move();
// minimal heartbeat API (updated by 0xF1 or 0x3E callbacks)
bool hasHeartbeat(uint32_t maxAgeMs) const;
// driver status (minimal; only "fault" is currently meaningful)
DriverStatus getDriverStatus() { return ready ? status : errorStatus; }
// RX callback (wired via CanPlus callbackRegisterMessage)
void requestStatusCallback(uint8_t data[8]);
private:
static inline uint8_t crc8(uint16_t id11, const uint8_t *data, size_t n) {
uint32_t sum = (uint32_t)(id11 & 0x7FF);
for (size_t i = 0; i < n; i++) sum += data[i];
return (uint8_t)(sum & 0xFF);
}
static inline int32_t clamp24(int32_t v) {
if (v > 8388607L) return 8388607L;
if (v < -8388608L) return -8388608L;
return v;
}
inline float pval(const AxisParameter &p) const {
const float v = p.value;
return isnan(v) ? p.valueDefault : v;
}
// stepsPerSecondToRpm unit: RPM per (steps/second)
inline uint16_t speedFromStepRate(float stepsPerSec) const {
if (stepsPerSec < 0.0F) stepsPerSec = -stepsPerSec;
const float scale = pval(stepsPerSecondToRpm);
float sF = stepsPerSec * scale;
if (sF < 0.0F) sF = 0.0F;
// MKS protocol speed range is 0..3000 RPM; keep the clamp aligned to that.
float capF = pval(maxRpm);
if (capF < 0.0F) capF = 0.0F;
if (capF > 3000.0F) capF = 3000.0F;
if (sF > capF) sF = capF;
return (uint16_t)lroundf(sF);
}
inline void sendWithCrc(const uint8_t *payload, size_t n); // n<=7
inline void sendF3(bool en);
inline void sendFE(int32_t pos, uint16_t speed);
// Always-on polling helpers
inline void sendF1(); // queryStatus (0xF1)
inline void send3E(); // readProtectState (0x3E)
void stopSyntheticMotion();
void resetToTrackingBaseline();
int canID = 0;
// reserved / future
int16_t model = 0;
int8_t statusMode = OFF;
uint8_t taskHandle = 0;
int stepSize = 1;
float currentFrequency = 0.0F;
unsigned long lastPeriod = 0;
float maxFrequency = HAL_FRACTIONAL_SEC;
volatile int absStep = 0;
void (*callback)() = nullptr;
bool useFastHardwareTimers = true;
bool isSlewing = false;
long lastTarget = LONG_MIN;
// --- always-on heartbeat / protect state ---
unsigned long lastStatusRequestTime = 0;
unsigned long lastProtectRequestTime = 0;
unsigned long lastStatusUpdateTime = 0;
unsigned long lastEnableTime = 0;
bool statusValid = false;
bool protectActive = false; // latched via 0x3E
// Minimal status model: only "fault" is currently meaningful
DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false };
// runtime adjustable settings
AxisParameter stepsPerSecondToRpm = {NAN, NAN, NAN, 0.0F, 10000.0F, AXP_FLOAT, "Steps/s to RPM"};
AxisParameter maxRpm = {NAN, NAN, NAN, 0.0F, 3000.0F, AXP_INTEGER, "Max Speed RPM"};
const int numParameters = 2;
};
#endif