diff --git a/changelog.md b/changelog.md
index cae8d7c..75d1785 100644
--- a/changelog.md
+++ b/changelog.md
@@ -1,5 +1,40 @@
## grblHAL changelog
+Build 20240812
+
+Core:
+
+* Improved handling of extended M commands \(plugin based\) command words. Fixes issues for programs containing extended M-codes using single meaning words \(which they as a rule should not do\).
+
+* Added core support for spindle encoder binding to spindles.
+
+* Added sorting of spindle report: enabled spindles are sorted first in order of spindle number, disabled by type then spindle id.
+
+* Changed realtime report to report spindle number instead of spindle id on changes in the `|S:` element. Part of fix for ioSender [issue #399](https://github.com/terjeio/ioSender/issues/399).
+
+Drivers:
+
+* imXRT1061, MSP432, STM32F4xx, STM32F7xx: Updated to take advandage of new spindle encoder binding functionality.
+
+Plugins:
+
+* Spindle: updated relevant drivers to use new spindle encoder binding functionality, simplified code. Fixes [issue #30](https://github.com/grblHAL/Plugins_spindle/issues/30).
+
+---
+
+Build 20240805
+
+Core:
+
+* Added function for getting speed \(RPM\) of stepper controlled by secondary stepper motor driver.
+
+Plugins:
+
+* Spindle: stepper spindle code now uses new function for getting speed \(RPM\) of motor.
+HAL functions for getting spindle data \(actual RPM, angular position etc.\) directed to stepper spindle code,
+
+---
+
Build 20240801
Core:
diff --git a/gcode.c b/gcode.c
index 6ac12ec..01d21b2 100644
--- a/gcode.c
+++ b/gcode.c
@@ -848,7 +848,7 @@ status_code_t gc_execute_block (char *block)
// Initialize command and value words and parser flags variables.
modal_groups_t command_words = {0}; // Bitfield for tracking G and M command words. Also used for modal group violations.
gc_parser_flags_t gc_parser_flags = {0}; // Parser flags for handling special cases.
- static parameter_words_t user_words = {0}; // User M-code words "taken"
+ parameter_words_t user_words = {0}; // User M-code words "taken"
// Determine if the line is a jogging motion or a normal g-code block.
if (block[0] == '$') { // NOTE: `$J=` already parsed when passed to this function.
@@ -1680,6 +1680,24 @@ status_code_t gc_execute_block (char *block)
// same way. If there is an explicit/implicit axis command, XYZ words are always used and are
// are removed at the end of error-checking.
+ // [0. User defined M commands ]:
+ if(command_words.M10 && gc_block.user_mcode) {
+
+ user_words.mask = gc_block.words.mask;
+ if((int_value = (uint_fast16_t)hal.user_mcode.validate(&gc_block, &gc_block.words)))
+ FAIL((status_code_t)int_value);
+ user_words.mask ^= gc_block.words.mask; // Flag "taken" words for execution
+
+ if(user_words.i)
+ ijk_words.i = Off;
+ if(user_words.j)
+ ijk_words.j = Off;
+ if(user_words.k)
+ ijk_words.k = Off;
+
+ axis_words.mask = 0;
+ }
+
// [1. Comments ]: MSG's may be supported by driver layer. Comment handling performed by protocol.
// [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate
@@ -1736,7 +1754,7 @@ status_code_t gc_execute_block (char *block)
// [4. Set spindle speed and address spindle ]: S or D is negative (done.)
if(gc_block.words.$) {
- bool single_spindle_only = (gc_block.words.s && !user_words.s) ||
+ bool single_spindle_only = gc_block.words.s ||
(command_words.G0 && (gc_block.modal.motion == MotionMode_SpindleSynchronized ||
gc_block.modal.motion == MotionMode_RigidTapping ||
gc_block.modal.motion == MotionMode_Threading)) ||
@@ -1779,11 +1797,11 @@ status_code_t gc_execute_block (char *block)
gc_state.modal.spindle.rpm_mode = gc_block.modal.spindle.rpm_mode;
}
- spindle_event = gc_block.words.s && !user_words.s;
+ spindle_event = gc_block.words.s;
- if (!gc_block.words.s)
+ if(!gc_block.words.s)
gc_block.values.s = gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_RPM ? gc_state.spindle.rpm : gc_state.spindle.hal->param->css.max_rpm;
- else if(!user_words.s && gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_CSS) {
+ else if(gc_state.modal.spindle.rpm_mode == SpindleSpeedMode_CSS) {
// Unsure what to do about S values when in SpindleSpeedMode_CSS - ignore? For now use it to (re)calculate surface speed.
// Reinsert commented out code above if this is removed!!
gc_block.values.s *= (gc_block.modal.units_imperial ? MM_PER_INCH * 12.0f : 1000.0f); // convert surface speed to mm/min
@@ -1796,9 +1814,9 @@ status_code_t gc_execute_block (char *block)
if(set_tool) { // M61
if(!gc_block.words.q)
FAIL(Status_GcodeValueWordMissing);
- if (floorf(gc_block.values.q) - gc_block.values.q != 0.0f)
+ if(!isintf(gc_block.values.q))
FAIL(Status_GcodeCommandValueNotInteger);
- if ((uint32_t)gc_block.values.q > (grbl.tool_table.n_tools ? grbl.tool_table.n_tools : MAX_TOOL_NUMBER))
+ if((uint32_t)gc_block.values.q > (grbl.tool_table.n_tools ? grbl.tool_table.n_tools : MAX_TOOL_NUMBER))
FAIL(Status_GcodeIllegalToolTableEntry);
gc_block.values.t = (uint32_t)gc_block.values.q;
@@ -1815,7 +1833,7 @@ status_code_t gc_execute_block (char *block)
}
}
#endif
- } else if (!gc_block.words.t)
+ } else if(!gc_block.words.t)
gc_block.values.t = gc_state.tool_pending;
if(command_words.M10 && port_command) {
@@ -1952,15 +1970,6 @@ status_code_t gc_execute_block (char *block)
}
}
- // [9a. User defined M commands ]:
- if (command_words.M10 && gc_block.user_mcode) {
- user_words.mask = gc_block.words.mask;
- if((int_value = (uint_fast16_t)hal.user_mcode.validate(&gc_block, &gc_block.words)))
- FAIL((status_code_t)int_value);
- user_words.mask ^= gc_block.words.mask; // Flag "taken" words for execution
- axis_words.mask = ijk_words.mask = 0;
- }
-
// [10. Dwell ]: P value missing. NOTE: See below.
if (gc_block.non_modal_command == NonModal_Dwell) {
if (!gc_block.words.p)
@@ -3071,7 +3080,7 @@ status_code_t gc_execute_block (char *block)
}
}
- if(!user_words.s && ((gc_state.spindle.rpm != gc_block.values.s) || gc_parser_flags.spindle_force_sync)) {
+ if(gc_state.spindle.rpm != gc_block.values.s || gc_parser_flags.spindle_force_sync) {
if(gc_state.modal.spindle.state.on && !gc_parser_flags.laser_is_motion) {
if(gc_block.spindle) {
gc_block.spindle->param->rpm = gc_block.values.s;
@@ -3215,7 +3224,7 @@ status_code_t gc_execute_block (char *block)
bool spindle_ok = false;
if(gc_block.spindle) {
if(grbl.on_spindle_programmed)
- grbl.on_spindle_programmed(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode);
+ grbl.on_spindle_programmed(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode);
if((spindle_ok = spindle_sync(gc_block.spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm)))
gc_block.spindle->param->state = gc_block.modal.spindle.state;
} else {
@@ -3224,7 +3233,7 @@ status_code_t gc_execute_block (char *block)
if(spindle_is_enabled(--idx)) {
spindle_ptrs_t *spindle = spindle_get(idx);
if(grbl.on_spindle_programmed)
- grbl.on_spindle_programmed(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode);
+ grbl.on_spindle_programmed(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm, gc_block.modal.spindle.rpm_mode);
if(spindle_sync(spindle, gc_block.modal.spindle.state, plan_data.spindle.rpm))
spindle->param->state = gc_block.modal.spindle.state;
else
diff --git a/grbl.h b/grbl.h
index 862dbb0..e4353db 100644
--- a/grbl.h
+++ b/grbl.h
@@ -42,7 +42,7 @@
#else
#define GRBL_VERSION "1.1f"
#endif
-#define GRBL_BUILD 20240801
+#define GRBL_BUILD 20240812
#define GRBL_URL "https://github.com/grblHAL"
diff --git a/modbus.h b/modbus.h
index c48a538..9368391 100644
--- a/modbus.h
+++ b/modbus.h
@@ -25,7 +25,7 @@
#define _MODBUS_H_
#ifndef MODBUS_MAX_ADU_SIZE
-#define MODBUS_MAX_ADU_SIZE 10
+#define MODBUS_MAX_ADU_SIZE 12
#endif
#ifndef MODBUS_QUEUE_LENGTH
#define MODBUS_QUEUE_LENGTH 8
diff --git a/report.c b/report.c
index 7a513fc..46e0805 100644
--- a/report.c
+++ b/report.c
@@ -333,6 +333,16 @@ static void report_help_message (void)
hal.stream.write("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $B ~ ! ? ctrl-x]" ASCII_EOL);
}
+// Prints plugin info.
+void report_plugin (const char *name, const char *version)
+{
+ hal.stream.write("[PLUGIN:");
+ hal.stream.write(name);
+ hal.stream.write(" v");
+ hal.stream.write(version);
+ hal.stream.write("]" ASCII_EOL);
+}
+
static bool report_group_settings (const setting_group_detail_t *groups, const uint_fast8_t n_groups, char *args)
{
bool found = false;
@@ -1088,7 +1098,6 @@ void report_build_info (char *line, bool extended)
}
}
-
// Prints the character string line grblHAL has received from the user, which has been pre-parsed,
// and has been sent into protocol_execute_line() routine to be executed by grblHAL.
void report_echo_line_received (char *line)
@@ -1098,6 +1107,15 @@ void report_echo_line_received (char *line)
hal.stream.write("]" ASCII_EOL);
}
+#if N_SYS_SPINDLE == 1 && N_SPINDLE > 1
+
+static void report_spindle_num (spindle_info_t *spindle, void *data)
+{
+ if(spindle->id == *((spindle_id_t *)data))
+ hal.stream.write_all(appendbuf(2, "|S:", uitoa((uint32_t)spindle->num)));
+}
+
+#endif
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
@@ -1246,7 +1264,7 @@ void report_realtime_status (void)
#elif N_SPINDLE > 1
if(report.spindle_id)
- hal.stream.write_all(appendbuf(2, "|S:", uitoa((uint32_t)spindle_0->id)));
+ spindle_enumerate_spindles(report_spindle_num, &spindle_0->id);
#endif
if(settings.status_report.pin_state) {
@@ -2516,9 +2534,56 @@ static void report_spindle (spindle_info_t *spindle, void *data)
}
}
+#if N_SPINDLE > 1
+
+typedef struct {
+ uint32_t idx;
+ uint32_t n_spindles;
+ spindle_info_t *spindles;
+} spindle_rdata_t;
+
+static void get_spindles (spindle_info_t *spindle, void *data)
+{
+ memcpy(&((spindle_rdata_t *)data)->spindles[((spindle_rdata_t *)data)->idx++], spindle, sizeof(spindle_info_t));
+}
+
+static int cmp_spindles (const void *a, const void *b)
+{
+ uint32_t key_a = ((spindle_info_t *)a)->num == -1 ? ((((spindle_info_t *)a)->hal->type + 1) << 8) | ((spindle_info_t *)a)->id : ((spindle_info_t *)a)->num,
+ key_b = ((spindle_info_t *)b)->num == -1 ? ((((spindle_info_t *)b)->hal->type + 1) << 8) | ((spindle_info_t *)b)->id : ((spindle_info_t *)b)->num;
+
+ return key_a - key_b;
+}
+
+#endif
+
status_code_t report_spindles (bool machine_readable)
{
- if(!spindle_enumerate_spindles(report_spindle, (void *)machine_readable) && !machine_readable)
+ bool has_spindles;
+
+#if N_SPINDLE > 1
+
+ spindle_rdata_t spindle_data = {0};
+
+ if((spindle_data.spindles = malloc(N_SPINDLE * sizeof(spindle_info_t)))) {
+
+ has_spindles = spindle_enumerate_spindles(get_spindles, &spindle_data);
+
+ spindle_data.n_spindles = spindle_data.idx;
+
+ qsort(spindle_data.spindles, spindle_data.n_spindles, sizeof(spindle_info_t), cmp_spindles);
+ for(spindle_data.idx = 0; spindle_data.idx < spindle_data.n_spindles; spindle_data.idx++)
+ report_spindle(&spindle_data.spindles[spindle_data.idx], (void *)machine_readable);
+
+ free(spindle_data.spindles);
+
+ } else
+
+#endif
+
+ has_spindles = spindle_enumerate_spindles(report_spindle, (void *)machine_readable);
+
+ if(!has_spindles && !machine_readable)
hal.stream.write("No spindles registered." ASCII_EOL);
return Status_OK;
diff --git a/report.h b/report.h
index b5b1b3a..1a7d99f 100644
--- a/report.h
+++ b/report.h
@@ -52,6 +52,8 @@ void report_warning (void *message);
// Prints Grbl help.
status_code_t report_help (char *args);
+void report_plugin (const char *name, const char *version);
+
// Prints Grbl settings
void report_grbl_settings (bool all, void *data);
diff --git a/spindle_control.c b/spindle_control.c
index 769c4f1..d2d19ec 100644
--- a/spindle_control.c
+++ b/spindle_control.c
@@ -51,6 +51,7 @@ typedef struct {
static uint8_t n_spindle = 0;
static spindle_sys_t sys_spindle[N_SYS_SPINDLE] = {0};
static spindle_reg_t spindles[N_SPINDLE] = {0}, *pwm_spindle = NULL;
+static const spindle_data_ptrs_t *encoder;
/*! \internal \brief Activates and registers a spindle as enabled with a specific spindle number.
\param spindle_id spindle id of spindle to activate as a \ref spindle_id_t.
@@ -96,20 +97,6 @@ static bool spindle_activate (spindle_id_t spindle_id, spindle_num_t spindle_num
memcpy(&spindle_hal, &spindle->hal, sizeof(spindle_ptrs_t));
- if(spindle->cfg->get_data == NULL) {
- if(settings.offset_lock.encoder_spindle == spindle_id) {
- spindle_hal.get_data = hal.spindle_data.get;
- spindle_hal.reset_data = hal.spindle_data.reset;
- if(!spindle->cfg->cap.at_speed)
- spindle_hal.cap.at_speed = !!spindle_hal.get_data;
- } else {
- spindle_hal.get_data = NULL;
- spindle_hal.reset_data = NULL;
- if(!spindle->cfg->cap.at_speed)
- spindle_hal.cap.at_speed = Off;
- }
- }
-
spindle_hal.cap.laser &= settings.mode == Mode_Laser;
if(grbl.on_spindle_select)
@@ -305,22 +292,68 @@ uint8_t spindle_get_count (void)
static spindle_num_t spindle_get_num (spindle_id_t spindle_id)
{
- uint_fast8_t idx = N_SPINDLE_SELECTABLE;
- spindle_num_t spindle_num = -1;
+ spindle_num_t spindle_num;
- const setting_detail_t *setting;
+ if((spindle_num = spindle_get_count() == 1 ? 0 : -1) == -1) {
- do {
- idx--;
- if((setting = setting_get_details(idx == 0 ? Setting_SpindleType : (setting_id_t)(Setting_SpindleEnable0 + idx), NULL))) {
- if(setting_get_int_value(setting, 0) == spindle_id)
- spindle_num = idx;
- }
- } while(idx && spindle_num == -1);
+ const setting_detail_t *setting;
+ uint_fast8_t idx = N_SPINDLE_SELECTABLE;
+
+ do {
+ idx--;
+ if((setting = setting_get_details(idx == 0 ? Setting_SpindleType : (setting_id_t)(Setting_SpindleEnable0 + idx), NULL))) {
+ if(setting_get_int_value(setting, 0) == spindle_id)
+ spindle_num = idx;
+ }
+ } while(idx && spindle_num == -1);
+ }
return spindle_num;
}
+void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data)
+{
+ uint_fast8_t idx;
+ spindle_ptrs_t *spindle;
+ spindle_num_t spindle_num;
+
+ encoder = encoder_data;
+
+ for(idx = 0; idx < n_spindle; idx++) {
+
+ spindle = spindle_get((spindle_num = spindle_get_num(idx)));
+
+ if(encoder_data && spindle_num == settings.offset_lock.encoder_spindle) {
+ spindles[idx].hal.get_data = encoder_data->get;
+ spindles[idx].hal.reset_data = encoder_data->reset;
+ spindles[idx].hal.cap.at_speed = spindles[idx].hal.cap.variable;
+ } else {
+ spindles[idx].hal.get_data = spindles[idx].cfg->get_data;
+ spindles[idx].hal.reset_data = spindles[idx].cfg->reset_data;
+ spindles[idx].hal.cap.at_speed = spindles[idx].cfg->cap.at_speed;
+ }
+
+ if(spindle) {
+ spindle->get_data = spindles[idx].hal.get_data;
+ spindle->reset_data = spindles[idx].hal.reset_data;
+ spindle->cap.at_speed = spindles[idx].hal.cap.at_speed;
+ }
+ }
+}
+
+bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm)
+{
+ spindle_data->rpm_programmed = rpm;
+ spindle_data->state_programmed.at_speed = false;
+
+ if((spindle_data->at_speed_enabled = spindle->at_speed_tolerance > 0.0f)) {
+ spindle_data->rpm_low_limit = rpm * (1.0f - (spindle->at_speed_tolerance / 100.0f));
+ spindle_data->rpm_high_limit = rpm * (1.0f + (spindle->at_speed_tolerance / 100.0f));
+ }
+
+ return spindle_data->at_speed_enabled;
+}
+
/*! \brief Enumerate registered spindles by calling a callback function for each of them.
\param callback pointer to a \ref spindle_enumerate_callback_ptr type function.
\param data pointer to optional data to pass to the callback function.
@@ -555,7 +588,7 @@ bool spindle_sync (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
if (!(ok = state_get() == STATE_CHECK_MODE)) {
- bool at_speed = !state.on || !spindle->cap.at_speed || settings.spindle.at_speed_tolerance <= 0.0f;
+ bool at_speed = !state.on || !spindle->cap.at_speed || spindle->at_speed_tolerance <= 0.0f;
// Empty planner buffer to ensure spindle is set when programmed.
if((ok = protocol_buffer_synchronize()) && set_state(spindle, state, rpm) && !at_speed) {
@@ -595,7 +628,7 @@ bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
if(state.on) {
if((ok = !spindle->cap.at_speed))
ok = delay_sec(settings.safety_door.spindle_on_delay, DelayMode_SysSuspend);
- else if((ok == (settings.spindle.at_speed_tolerance <= 0.0f))) {
+ else if((ok == (spindle->at_speed_tolerance <= 0.0f))) {
float delay = 0.0f;
while(!(ok = spindle->get_state(spindle).at_speed)) {
if(!(ok = delay_sec(0.1f, DelayMode_SysSuspend)))
@@ -624,13 +657,7 @@ float spindle_set_rpm (spindle_ptrs_t *spindle, float rpm, override_t override_p
if(override_pct != 100)
rpm *= 0.01f * (float)override_pct; // Scale RPM by override value.
- // Apply RPM limits
- if (rpm <= 0.0f) // TODO: remove this test?
- rpm = 0.0f;
- else if (rpm > spindle->rpm_max)
- rpm = spindle->rpm_max;
- else if (rpm < spindle->rpm_min)
- rpm = spindle->rpm_min;
+ rpm = rpm <= 0.0f ? 0.0f : constrain(rpm, spindle->rpm_min, spindle->rpm_max);
spindle->param->rpm_overridden = rpm;
spindle->param->override_pct = override_pct;
@@ -756,6 +783,7 @@ bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_
pwm_data->settings = settings;
spindle->rpm_min = pwm_data->rpm_min = settings->rpm_min;
spindle->rpm_max = settings->rpm_max;
+ spindle->at_speed_tolerance = settings->at_speed_tolerance;
spindle->cap.rpm_range_locked = On;
if((spindle->cap.variable = !settings->flags.pwm_disable && spindle->rpm_max > spindle->rpm_min)) {
diff --git a/spindle_control.h b/spindle_control.h
index 889a9e8..471762e 100644
--- a/spindle_control.h
+++ b/spindle_control.h
@@ -98,6 +98,7 @@ typedef struct {
uint32_t index_count;
uint32_t pulse_count;
uint32_t error_count;
+ bool at_speed_enabled;
spindle_state_t state_programmed;
} spindle_data_t;
@@ -208,14 +209,14 @@ typedef struct {
float rpm_max;
float rpm_min;
float pwm_freq;
- float pwm_period; // currently unused
+ float pwm_period; // currently unused
float pwm_off_value;
float pwm_min_value;
float pwm_max_value;
- float at_speed_tolerance;
+ float at_speed_tolerance; //!< Tolerance in percent of programmed speed.
pwm_piece_t pwm_piece[SPINDLE_NPWM_PIECES];
pid_values_t pid;
- uint16_t ppr; // Spindle encoder pulses per revolution
+ uint16_t ppr; //!< Spindle encoder pulses per revolution (PPR).
spindle_state_t invert;
spindle_settings_flags_t flags;
} spindle_settings_t;
@@ -273,6 +274,7 @@ struct spindle_ptrs {
uint_fast16_t pwm_off_value; //!< Value for switching PWM signal off.
float rpm_min; //!< Minimum spindle RPM.
float rpm_max; //!< Maximum spindle RPM.
+ float at_speed_tolerance; //!< Tolerance in percent of programmed speed.
spindle_config_ptr config; //!< Optional handler for configuring the spindle.
spindle_set_state_ptr set_state; //!< Handler for setting spindle state.
spindle_get_state_ptr get_state; //!< Handler for getting spindle state.
@@ -350,6 +352,15 @@ void spindle_all_off (void);
// The following functions are not called by the core, may be called by driver code.
//
+#define spindle_validate_at_speed(d, r) { d.rpm = r; d.state_programmed.at_speed = !d.at_speed_enabled || (d.rpm >= d.rpm_low_limit && d.rpm <= d.rpm_high_limit); }
+/*
+__attribute__((always_inline)) static inline void spindle_validate_at_speed (spindle_data_t *spindle_data, float rpm)
+{
+ spindle_data->rpm = rpm;
+ spindle_data->state_programmed.at_speed = !spindle_data->at_speed_enabled || (spindle_data->rpm >= spindle_data->rpm_low_limit && spindle_data->rpm <= spindle_data->rpm_high_limit);
+}
+*/
+
bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_data, spindle_settings_t *settings, uint32_t clock_hz);
spindle_id_t spindle_register (const spindle_ptrs_t *spindle, const char *name);
@@ -364,6 +375,9 @@ spindle_cap_t spindle_get_caps (bool active);
void spindle_update_caps (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_caps);
+void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data);
+
+bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm);
spindle_ptrs_t *spindle_get_hal (spindle_id_t spindle_id, spindle_hal_t hal);
diff --git a/stepper2.c b/stepper2.c
index 894b60b..ea2c082 100644
--- a/stepper2.c
+++ b/stepper2.c
@@ -272,6 +272,15 @@ st2_motor_t *st2_motor_init (uint_fast8_t axis_idx, bool is_spindle)
return motor;
}
+/*! \brief Get current speed (RPM).
+\param motor pointer to a \a st2_motor structure.
+\returns current speed in RPM.
+*/
+float st2_get_speed (st2_motor_t *motor)
+{
+ return motor->state == State_Idle ? 0.0f : 60.0f / ((float)motor->delay * settings.axis[motor->idx].steps_per_mm / 1000000.0f);
+}
+
/*! \brief Set speed.
Change speed of a running motor. Typically used for motors bound as a spindle.
diff --git a/stepper2.h b/stepper2.h
index 8f3dc01..d5984ca 100644
--- a/stepper2.h
+++ b/stepper2.h
@@ -33,6 +33,7 @@ typedef struct st2_motor st2_motor_t;
st2_motor_t *st2_motor_init (uint_fast8_t axis_idx, bool is_spindle);
bool st2_motor_bind_spindle (uint_fast8_t axis_idx);
+float st2_get_speed (st2_motor_t *motor);
float st2_motor_set_speed (st2_motor_t *motor, float speed);
bool st2_motor_move (st2_motor_t *motor, const float move, const float speed, position_t type);
bool st2_motor_run (st2_motor_t *motor);