diff --git a/modbus.c b/modbus.c
index 8b2a6bf..02baf4a 100644
--- a/modbus.c
+++ b/modbus.c
@@ -244,6 +244,11 @@ static void modbus_poll (void)
}
break;
+ case ModBus_Timeout:
+ state = ModBus_Silent;
+ silence_until = ms + silence_timeout;
+ break;
+
default:
break;
}
diff --git a/shared.h b/shared.h
index 756be13..0512a01 100644
--- a/shared.h
+++ b/shared.h
@@ -32,8 +32,8 @@
#define SPINDLE_ALL -1
#define SPINDLE_HUANYANG1 1
#define SPINDLE_HUANYANG2 2
-#define SPINDLE_GS20 3 // To be added
-#define SPINDLE_YL620A 4 // To be added
+#define SPINDLE_GS20 3
+#define SPINDLE_YL620A 4
#define SPINDLE_MODVFD 5
#define SPINDLE_H100 6 // Not tested
diff --git a/vfd/gs20.c b/vfd/gs20.c
new file mode 100644
index 0000000..dafdbce
--- /dev/null
+++ b/vfd/gs20.c
@@ -0,0 +1,299 @@
+/*
+ gs20.c - GS20 VFD spindle support
+
+ Part of grblHAL
+
+ Copyright (c) 2022 Andrew Marles
+ Copyright (c) 2022 Terje Io
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see .
+
+*/
+
+#include "../shared.h"
+
+#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_GS20
+
+#include
+#include
+
+#include "spindle.h"
+
+static spindle_id_t vfd_spindle_id;
+static float rpm_programmed = -1.0f;
+static spindle_state_t vfd_state = {0};
+static spindle_data_t spindle_data = {0};
+static uint32_t rpm_max = 0;
+static uint16_t retry_counter = 0;
+static on_report_options_ptr on_report_options;
+static on_spindle_select_ptr on_spindle_select;
+static driver_reset_ptr driver_reset;
+
+static void gs20_rx_packet (modbus_message_t *msg);
+static void gs20_rx_exception (uint8_t code, void *context);
+
+static const modbus_callbacks_t callbacks = {
+ .on_rx_packet = gs20_rx_packet,
+ .on_rx_exception = gs20_rx_exception
+};
+
+// To-do, this should be a mechanism to read max RPM from the VFD in order to configure RPM/Hz instead of above define.
+bool gs20_spindle_config (void)
+{
+ modbus_set_silence(NULL);
+
+ return 1;
+}
+
+static void spindleSetRPM (float rpm, bool block)
+{
+ uint16_t data = ((uint32_t)(rpm) * 50) / vfd_config.vfd_rpm_hz;
+
+ modbus_message_t rpm_cmd = {
+ .context = (void *)VFD_SetRPM,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_WriteRegister,
+ .adu[2] = 0x20,
+ .adu[3] = 0x01,
+ .adu[4] = data >> 8,
+ .adu[5] = data & 0xFF,
+ .tx_length = 8,
+ .rx_length = 8
+ };
+
+ vfd_state.at_speed = false;
+
+ modbus_send(&rpm_cmd, &callbacks, block);
+
+ if(settings.spindle.at_speed_tolerance > 0.0f) {
+ spindle_data.rpm_low_limit = rpm / (1.0f + settings.spindle.at_speed_tolerance);
+ spindle_data.rpm_high_limit = rpm * (1.0f + settings.spindle.at_speed_tolerance);
+ }
+ rpm_programmed = rpm;
+}
+
+void gs20_spindleUpdateRPM (float rpm)
+{
+ spindleSetRPM(rpm, false);
+}
+
+// Start or stop spindle
+void gs20_spindleSetState (spindle_state_t state, float rpm)
+{
+ uint8_t runstop = 0;
+ uint8_t direction = 0;
+
+ if(!state.on || rpm == 0.0f)
+ runstop = 0x1;
+ else
+ runstop = 0x2;
+
+ if(state.ccw)
+ direction = 0x20;
+ else
+ direction = 0x10;
+
+ modbus_message_t mode_cmd = {
+ .context = (void *)VFD_SetStatus,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_WriteRegister,
+ .adu[2] = 0x20,
+ .adu[3] = 0x00,
+ .adu[4] = 0x00,
+ .adu[5] = direction|runstop,
+ .tx_length = 8,
+ .rx_length = 8
+ };
+
+ if(vfd_state.ccw != state.ccw)
+ rpm_programmed = 0.0f;
+
+ vfd_state.on = state.on;
+ vfd_state.ccw = state.ccw;
+
+ if(modbus_send(&mode_cmd, &callbacks, true))
+ spindleSetRPM(rpm, true);
+}
+
+static spindle_data_t *gs20_spindleGetData (spindle_data_request_t request)
+{
+ return &spindle_data;
+}
+
+// Returns spindle state in a spindle_state_t variable
+spindle_state_t gs20_spindleGetState (void)
+{
+ static uint32_t last_ms;
+ uint32_t ms = hal.get_elapsed_ticks();
+
+ modbus_message_t mode_cmd = {
+ .context = (void *)VFD_GetRPM,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_ReadHoldingRegisters,
+ .adu[2] = 0x21,
+ .adu[3] = 0x03,
+ .adu[4] = 0x00,
+ .adu[5] = 0x01,
+ .tx_length = 8,
+ .rx_length = 7
+ };
+
+ if(ms > (last_ms + VFD_RETRY_DELAY)){ //don't spam the port
+ modbus_send(&mode_cmd, &callbacks, false); // TODO: add flag for not raising alarm?
+ last_ms = ms;
+ }
+
+ // Get the actual RPM from spindle encoder input when available.
+ if(hal.spindle.get_data && hal.spindle.get_data != gs20_spindleGetData) {
+ float rpm = hal.spindle.get_data(SpindleData_RPM)->rpm;
+ vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (rpm >= spindle_data.rpm_low_limit && rpm <= spindle_data.rpm_high_limit);
+ }
+
+ return vfd_state; // return previous state as we do not want to wait for the response
+}
+
+static void gs20_rx_packet (modbus_message_t *msg)
+{
+ if(!(msg->adu[0] & 0x80)) {
+
+ switch((vfd_response_t)msg->context) {
+
+ case VFD_GetRPM:
+ spindle_data.rpm = (float)((msg->adu[3] << 8) | msg->adu[4]) * vfd_config.vfd_rpm_hz / 100;
+ vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (spindle_data.rpm >= spindle_data.rpm_low_limit && spindle_data.rpm <= spindle_data.rpm_high_limit);
+ retry_counter = 0;
+ break;
+
+ case VFD_GetMaxRPM:
+ rpm_max = (msg->adu[4] << 8) | msg->adu[5];
+ retry_counter = 0;
+ break;
+
+ case VFD_SetStatus:
+ retry_counter = 0;
+ break;
+
+ case VFD_SetRPM:
+ retry_counter = 0;
+ break;
+
+ default:
+ retry_counter = 0;
+ break;
+ }
+ }
+}
+
+static void raise_alarm (uint_fast16_t state)
+{
+ system_raise_alarm(Alarm_Spindle);
+}
+
+static void gs20_rx_exception (uint8_t code, void *context)
+{
+ // Alarm needs to be raised directly to correctly handle an error during reset (the rt command queue is
+ // emptied on a warm reset). Exception is during cold start, where alarms need to be queued.
+ if(sys.cold_start) {
+ protocol_enqueue_rt_command(raise_alarm);
+ }
+ //if RX exceptions during one of the VFD messages, need to retry.
+ else if ((vfd_response_t)context > 0 ) {
+ retry_counter++;
+ if (retry_counter >= VFD_RETRIES) {
+ system_raise_alarm(Alarm_Spindle);
+ retry_counter = 0;
+ return;
+ }
+
+ switch((vfd_response_t)context) {
+
+ case VFD_SetStatus:
+ case VFD_SetRPM:
+// modbus_reset();
+ hal.spindle.set_state(hal.spindle.get_state(), sys.spindle_rpm);
+ break;
+
+ case VFD_GetRPM:
+// modbus_reset();
+ hal.spindle.get_state();
+ break;
+
+ default:
+ break;
+ }//close switch statement
+ } else {
+ retry_counter = 0;
+ system_raise_alarm(Alarm_Spindle);
+ }
+}
+
+void gs20_onReportOptions (bool newopt)
+{
+ on_report_options(newopt);
+
+ if(!newopt) {
+ hal.stream.write("[PLUGIN:Durapulse VFD GS20 v0.03]" ASCII_EOL);
+ }
+}
+
+void gs20_reset (void)
+{
+ driver_reset();
+}
+
+bool gs20_spindle_select (spindle_id_t spindle_id)
+{
+ if(spindle_id == vfd_spindle_id) {
+
+ if(settings.spindle.ppr == 0)
+ hal.spindle.get_data = gs20_spindleGetData;
+
+ } else if(hal.spindle.get_data == gs20_spindleGetData)
+ hal.spindle.get_data = NULL;
+
+ if(on_spindle_select)
+ on_spindle_select(spindle_id);
+
+ return true;
+}
+
+void vfd_gs20_init (void)
+{
+ static const vfd_spindle_ptrs_t spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = gs20_spindle_config,
+ .spindle.set_state = gs20_spindleSetState,
+ .spindle.get_state = gs20_spindleGetState,
+ .spindle.update_rpm = gs20_spindleUpdateRPM
+ };
+
+ if((vfd_spindle_id = vfd_register(&spindle, "Durapulse GS20")) != -1) {
+
+ on_spindle_select = grbl.on_spindle_select;
+ grbl.on_spindle_select = gs20_spindle_select;
+
+ on_report_options = grbl.on_report_options;
+ grbl.on_report_options = gs20_onReportOptions;
+
+ //driver_reset = hal.driver_reset;
+ //hal.driver_reset = gs20_reset;
+ }
+}
+
+#endif
diff --git a/vfd/h100.c b/vfd/h100.c
index 524407b..6fdbaec 100644
--- a/vfd/h100.c
+++ b/vfd/h100.c
@@ -254,14 +254,14 @@ static bool vfd_spindle_select (spindle_id_t spindle_id)
void vfd_h100_init (void)
{
- static const spindle_ptrs_t spindle = {
- .cap.variable = On,
- .cap.at_speed = On,
- .cap.direction = On,
- .config = vfd_spindle_config,
- .set_state = spindleSetState,
- .get_state = spindleGetState,
- .update_rpm = spindleUpdateRPM
+ static const vfd_spindle_ptrs_t spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = vfd_spindle_config,
+ .spindle.set_state = spindleSetState,
+ .spindle.get_state = spindleGetState,
+ .spindle.update_rpm = spindleUpdateRPM
};
if((vfd_spindle_id = vfd_register(&spindle, "H-100")) != -1) {
diff --git a/vfd/huanyang.c b/vfd/huanyang.c
index 333dd1f..8d09152 100644
--- a/vfd/huanyang.c
+++ b/vfd/huanyang.c
@@ -428,8 +428,8 @@ static bool huanyang_spindle_select (spindle_id_t spindle_id)
} else if(hal.spindle.get_data == spindleGetData)
hal.spindle.get_data = NULL;
- if(on_spindle_select && on_spindle_select(spindle_id))
- return true;
+ if(on_spindle_select)
+ on_spindle_select(spindle_id);
return true;
}
@@ -437,28 +437,28 @@ static bool huanyang_spindle_select (spindle_id_t spindle_id)
void vfd_huanyang_init (void)
{
#if VFD_ENABLE == SPINDLE_HUANYANG1 || VFD_ENABLE == SPINDLE_ALL
- static const spindle_ptrs_t v1_spindle = {
- .cap.variable = On,
- .cap.at_speed = On,
- .cap.direction = On,
- .config = v1_spindle_config,
- .set_state = v1_spindleSetState,
- .get_state = v1_spindleGetState,
- .update_rpm = v1_spindleUpdateRPM
+ static const vfd_spindle_ptrs_t v1_spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = v1_spindle_config,
+ .spindle.set_state = v1_spindleSetState,
+ .spindle.get_state = v1_spindleGetState,
+ .spindle.update_rpm = v1_spindleUpdateRPM
};
v1_spindle_id = vfd_register(&v1_spindle, "Huanyang v1");
#endif
#if VFD_ENABLE == SPINDLE_HUANYANG2 || VFD_ENABLE == SPINDLE_ALL
- static const spindle_ptrs_t v2_spindle = {
- .cap.variable = On,
- .cap.at_speed = On,
- .cap.direction = On,
- .config = v2_spindle_config,
- .set_state = v2_spindleSetState,
- .get_state = v2_spindleGetState,
- .update_rpm = v2_spindleUpdateRPM
+ static const vfd_spindle_ptrs_t v2_spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = v2_spindle_config,
+ .spindle.set_state = v2_spindleSetState,
+ .spindle.get_state = v2_spindleGetState,
+ .spindle.update_rpm = v2_spindleUpdateRPM
};
v2_spindle_id = vfd_register(&v2_spindle, "Huanyang P2A");
diff --git a/vfd/modvfd.c b/vfd/modvfd.c
index 162d92b..62fecfa 100644
--- a/vfd/modvfd.c
+++ b/vfd/modvfd.c
@@ -1,6 +1,7 @@
/*
vfd/modvfd.c - MODVFD VFD spindle support for Modbus RTU compatible spindles.
+
Requires Modbus RTU support with 8N1 configuration.
Part of grblHAL
@@ -248,7 +249,7 @@ void OnReportOptions (bool newopt)
void modvfd_reset (void)
{
-
+ driver_reset();
}
bool modvfd_spindle_select (spindle_id_t spindle_id)
@@ -269,14 +270,14 @@ bool modvfd_spindle_select (spindle_id_t spindle_id)
void vfd_modvfd_init (void)
{
- static const spindle_ptrs_t spindle = {
- .cap.variable = On,
- .cap.at_speed = On,
- .cap.direction = On,
- .config = modvfd_spindle_config,
- .set_state = modvfd_spindleSetState,
- .get_state = modvfd_spindleGetState,
- .update_rpm = modvfd_spindleUpdateRPM
+ static const vfd_spindle_ptrs_t spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = modvfd_spindle_config,
+ .spindle.set_state = modvfd_spindleSetState,
+ .spindle.get_state = modvfd_spindleGetState,
+ .spindle.update_rpm = modvfd_spindleUpdateRPM
};
if((vfd_spindle_id = vfd_register(&spindle, "MODVFD")) != -1) {
@@ -287,8 +288,8 @@ void vfd_modvfd_init (void)
on_report_options = grbl.on_report_options;
grbl.on_report_options = OnReportOptions;
- driver_reset = hal.driver_reset;
- hal.driver_reset = modvfd_reset;
+ //driver_reset = hal.driver_reset;
+ //hal.driver_reset = modvfd_reset;
}
}
#endif
diff --git a/vfd/spindle.c b/vfd/spindle.c
index cfd6089..c4803c0 100644
--- a/vfd/spindle.c
+++ b/vfd/spindle.c
@@ -43,26 +43,50 @@
typedef struct {
spindle_id_t id;
- const spindle_ptrs_t *spindle;
+ const vfd_spindle_ptrs_t *vfd;
} vfd_spindle_t;
static uint8_t n_spindle = 0;
+static bool spindle_changed = false;
+static vfd_ptrs_t vfd_spindle = {0};
static vfd_spindle_t vfd_spindles[N_SPINDLE];
static nvs_address_t nvs_address = 0;
static on_spindle_select_ptr on_spindle_select;
-static on_report_options_ptr on_report_options;
+static on_realtime_report_ptr on_realtime_report = NULL;
vfd_settings_t vfd_config;
-spindle_id_t vfd_register (const spindle_ptrs_t *spindle, const char *name)
+static void vfd_realtime_report (stream_write_ptr stream_write, report_tracking_flags_t report)
+{
+ static float load = -1.0f;
+
+ if(on_realtime_report)
+ on_realtime_report(stream_write, report);
+
+ if(vfd_spindle.get_load) {
+ float new_load = vfd_spindle.get_load();
+ if(load != new_load || spindle_changed || report.all) {
+ load = new_load;
+ spindle_changed = false;
+ stream_write("|Sl:");
+ stream_write(ftoa(load, 1));
+ }
+ }
+}
+
+spindle_id_t vfd_register (const vfd_spindle_ptrs_t *vfd, const char *name)
{
spindle_id_t spindle_id = -1;
- if(n_spindle < N_SPINDLE && (spindle_id = spindle_register(spindle, name)) != -1) {
+ if(n_spindle < N_SPINDLE && (spindle_id = spindle_register(&vfd->spindle, name)) != -1) {
vfd_spindles[n_spindle].id = spindle_id;
- vfd_spindles[n_spindle++].spindle = spindle;
- return n_spindle - 1;
+ vfd_spindles[n_spindle++].vfd = vfd;
+
+ if(vfd->vfd.get_load && on_realtime_report == NULL) {
+ on_realtime_report = grbl.on_realtime_report;
+ grbl.on_realtime_report = vfd_realtime_report;
+ }
}
return spindle_id;
@@ -161,37 +185,31 @@ static setting_details_t vfd_setting_details = {
.save = vfd_settings_save
};
-/*
-static void vfd_onReportOptions (bool newopt)
-{
- on_report_options(newopt);
-
- if(!newopt) {
- hal.stream.write("[PLUGIN:VFD SELECTOR v0.02]" ASCII_EOL);
- }
-}
-
static bool vfd_spindle_select (spindle_id_t spindle_id)
{
- bool select_ok = false;
+ uint_fast8_t idx = n_spindle;
+ spindle_changed = true;
+ memset(&vfd_spindle, 0, sizeof(vfd_ptrs_t));;
+ if(n_spindle) do {
+ if(vfd_spindles[--idx].id == spindle_id) {
+ memcpy(&vfd_spindle, &vfd_spindles[idx].vfd->vfd, sizeof(vfd_ptrs_t));
+ break;
+ }
+ } while(idx);
- if(select_ok && on_spindle_select && on_spindle_select(spindle_id))
- return true;
+ if(on_spindle_select)
+ on_spindle_select(spindle_id);
- return select_ok;
+ return true;
}
-*/
void vfd_init (void)
{
if(modbus_enabled() && (nvs_address = nvs_alloc(sizeof(vfd_settings_t)))) {
on_spindle_select = grbl.on_spindle_select;
-// grbl.on_spindle_select = vfd_spindle_select;
-
- on_report_options = grbl.on_report_options;
-// grbl.on_report_options = vfd_onReportOptions;
+ grbl.on_spindle_select = vfd_spindle_select;
settings_register(&vfd_setting_details);
@@ -200,10 +218,25 @@ void vfd_init (void)
vfd_huanyang_init();
#endif
+#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_GS20
+ extern void vfd_gs20_init (void);
+ vfd_gs20_init();
+#endif
+
+#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_YL620A
+ extern void vfd_yl620_init (void);
+ vfd_yl620_init();
+#endif
+
#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_MODVFD
extern void vfd_modvfd_init (void);
vfd_modvfd_init();
#endif
+
+#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_H100
+ extern void vfd_h100_init (void);
+ vfd_h100_init();
+#endif
}
}
diff --git a/vfd/spindle.h b/vfd/spindle.h
index c4be6a1..b31e618 100644
--- a/vfd/spindle.h
+++ b/vfd/spindle.h
@@ -73,8 +73,19 @@ typedef struct {
float out_divider;
} vfd_settings_t;
+typedef float (*vfd_get_load_ptr)(void);
+
+typedef struct {
+ vfd_get_load_ptr get_load;
+} vfd_ptrs_t;
+
+typedef struct {
+ const spindle_ptrs_t spindle;
+ const vfd_ptrs_t vfd;
+} vfd_spindle_ptrs_t;
+
extern vfd_settings_t vfd_config;
-spindle_id_t vfd_register (const spindle_ptrs_t *spindle, const char *name);
+spindle_id_t vfd_register (const vfd_spindle_ptrs_t *vfd, const char *name);
#endif
diff --git a/vfd/yl620.c b/vfd/yl620.c
new file mode 100644
index 0000000..e7dab28
--- /dev/null
+++ b/vfd/yl620.c
@@ -0,0 +1,347 @@
+/*
+ yl620.c - Yalang VFD spindle support
+
+ Part of grblHAL
+
+ Copyright (c) 2022 Andrew Marles
+ Copyright (c) 2022 Terje Io
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see .
+
+*/
+
+/*
+ Manual Configuration required for the YL620
+ Parameter number Description Value
+ -------------------------------------------------------------------------------
+ P00.00 Main frequency 400.00Hz (match to your spindle)
+ P00.01 Command source 3
+
+ P03.00 RS485 Baud rate 3 (9600)
+ P03.01 RS485 address 1
+ P03.02 RS485 protocol 2
+ P03.08 Frequency given lower limit 100.0Hz (match to your spindle cooling-type)
+ ===============================================================================================================
+ RS485 communication is standard Modbus RTU
+ Therefore, the following operation codes are relevant:
+ 0x03: read single holding register
+ 0x06: write single holding register
+ Given a parameter Pnn.mm, the high byte of the register address is nn,
+ the low is mm. The numbers nn and mm in the manual are given in decimal,
+ so P13.16 would be register address 0x0d10 when represented in hex.
+ Holding register address Description
+ ---------------------------------------------------------------------------
+ 0x0000 main frequency
+ 0x0308 frequency given lower limit
+ 0x2000 command register (further information below)
+ 0x2001 Modbus485 frequency command (x0.1Hz => 2500 = 250.0Hz)
+ 0x200A Target frequency
+ 0x200B Output frequency
+ 0x200C Output current
+ Command register at holding address 0x2000
+ --------------------------------------------------------------------------
+ bit 1:0 b00: No function
+ b01: shutdown command
+ b10: start command
+ b11: Jog command
+ bit 3:2 reserved
+ bit 5:4 b00: No function
+ b01: Forward command
+ b10: Reverse command
+ b11: change direction
+ bit 7:6 b00: No function
+ b01: reset an error flag
+ b10: reset all error flags
+ b11: reserved
+*/
+
+#include "../shared.h"
+
+#if VFD_ENABLE == SPINDLE_ALL || VFD_ENABLE == SPINDLE_YL620A
+
+#include
+#include
+
+#include "spindle.h"
+
+static spindle_id_t vfd_spindle_id;
+static float rpm_programmed = -1.0f;
+static spindle_state_t vfd_state = {0};
+static spindle_data_t spindle_data = {0};
+static uint32_t rpm_max = 0;
+static uint16_t retry_counter = 0;
+static on_report_options_ptr on_report_options;
+static on_spindle_select_ptr on_spindle_select;
+static driver_reset_ptr driver_reset;
+
+static void rx_packet (modbus_message_t *msg);
+static void rx_exception (uint8_t code, void *context);
+
+static const modbus_callbacks_t callbacks = {
+ .on_rx_packet = rx_packet,
+ .on_rx_exception = rx_exception
+};
+
+// To-do, this should be a mechanism to read max RPM from the VFD in order to configure RPM/Hz instead of above define.
+bool yl620_spindle_config (void)
+{
+ modbus_set_silence(NULL);
+
+ return 1;
+}
+
+static void spindleSetRPM (float rpm, bool block)
+{
+ uint16_t data = ((uint32_t)(rpm) * 10) / vfd_config.vfd_rpm_hz;
+
+ modbus_message_t rpm_cmd = {
+ .context = (void *)VFD_SetRPM,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_WriteRegister,
+ .adu[2] = 0x20,
+ .adu[3] = 0x01,
+ .adu[4] = data >> 8,
+ .adu[5] = data & 0xFF,
+ .tx_length = 8,
+ .rx_length = 8
+ };
+
+ vfd_state.at_speed = false;
+
+ modbus_send(&rpm_cmd, &callbacks, block);
+
+ if(settings.spindle.at_speed_tolerance > 0.0f) {
+ spindle_data.rpm_low_limit = rpm / (1.0f + settings.spindle.at_speed_tolerance);
+ spindle_data.rpm_high_limit = rpm * (1.0f + settings.spindle.at_speed_tolerance);
+ }
+ rpm_programmed = rpm;
+}
+
+void yl620_spindleUpdateRPM (float rpm)
+{
+ spindleSetRPM(rpm, false);
+}
+
+// Start or stop spindle
+void yl620_spindleSetState (spindle_state_t state, float rpm)
+{
+ uint8_t runstop = 0;
+ uint8_t direction = 0;
+
+ if(!state.on || rpm == 0.0f)
+ runstop = 0x1;
+ else
+ runstop = 0x2;
+
+ if(state.ccw)
+ direction = 0x20;
+ else
+ direction = 0x10;
+
+ modbus_message_t mode_cmd = {
+ .context = (void *)VFD_SetStatus,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_WriteRegister,
+ .adu[2] = 0x20,
+ .adu[3] = 0x00,
+ .adu[4] = 0x00,
+ .adu[5] = direction|runstop,
+ .tx_length = 8,
+ .rx_length = 8
+ };
+
+ if(vfd_state.ccw != state.ccw)
+ rpm_programmed = 0.0f;
+
+ vfd_state.on = state.on;
+ vfd_state.ccw = state.ccw;
+
+ if(modbus_send(&mode_cmd, &callbacks, true))
+ spindleSetRPM(rpm, true);
+}
+
+static spindle_data_t *spindleGetData (spindle_data_request_t request)
+{
+ return &spindle_data;
+}
+
+// Returns spindle state in a spindle_state_t variable
+spindle_state_t yl620_spindleGetState (void)
+{
+
+ static uint32_t last_ms;
+ uint32_t ms = hal.get_elapsed_ticks();
+
+ modbus_message_t mode_cmd = {
+ .context = (void *)VFD_GetRPM,
+ .crc_check = false,
+ .adu[0] = vfd_config.modbus_address,
+ .adu[1] = ModBus_ReadHoldingRegisters,
+ .adu[2] = 0x20,
+ .adu[3] = 0x0B,
+ .adu[4] = 0x00,
+ .adu[5] = 0x01,
+ .tx_length = 8,
+ .rx_length = 7
+ };
+
+ if(ms > (last_ms + VFD_RETRY_DELAY)){ //don't spam the port
+ modbus_send(&mode_cmd, &callbacks, false); // TODO: add flag for not raising alarm?
+ last_ms = ms;
+ }
+
+ // Get the actual RPM from spindle encoder input when available.
+ if(hal.spindle.get_data && hal.spindle.get_data != spindleGetData) {
+ float rpm = hal.spindle.get_data(SpindleData_RPM)->rpm;
+ vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (rpm >= spindle_data.rpm_low_limit && rpm <= spindle_data.rpm_high_limit);
+ }
+
+ return vfd_state; // return previous state as we do not want to wait for the response
+}
+
+static void rx_packet (modbus_message_t *msg)
+{
+ if(!(msg->adu[0] & 0x80)) {
+
+ switch((vfd_response_t)msg->context) {
+
+ case VFD_GetRPM:
+ spindle_data.rpm = (float)((msg->adu[3] << 8) | msg->adu[4]) * vfd_config.vfd_rpm_hz / 10;
+ vfd_state.at_speed = settings.spindle.at_speed_tolerance <= 0.0f || (spindle_data.rpm >= spindle_data.rpm_low_limit && spindle_data.rpm <= spindle_data.rpm_high_limit);
+ retry_counter = 0;
+ break;
+
+ case VFD_GetMaxRPM:
+ rpm_max = (msg->adu[4] << 8) | msg->adu[5];
+ retry_counter = 0;
+ break;
+
+ case VFD_SetStatus:
+ //add check here to ensure command was successful, retry if not.
+ retry_counter = 0;
+ break;
+
+ case VFD_SetRPM:
+ //add check here to ensure command was successful, retry if not.
+ retry_counter = 0;
+ break;
+
+ default:
+ retry_counter = 0;
+ break;
+ }
+ }
+}
+
+static void raise_alarm (uint_fast16_t state)
+{
+ system_raise_alarm(Alarm_Spindle);
+}
+
+static void rx_exception (uint8_t code, void *context)
+{
+ // Alarm needs to be raised directly to correctly handle an error during reset (the rt command queue is
+ // emptied on a warm reset). Exception is during cold start, where alarms need to be queued.
+ if(sys.cold_start){
+ protocol_enqueue_rt_command(raise_alarm);
+ }
+ //if RX exceptions during one of the VFD messages, need to retry.
+ else if ((vfd_response_t)context > 0) {
+ retry_counter++;
+ if (retry_counter >= VFD_RETRIES) {
+ system_raise_alarm(Alarm_Spindle);
+ retry_counter = 0;
+ return;
+ }
+
+ switch((vfd_response_t)context) {
+ case VFD_SetStatus:
+ case VFD_SetRPM:
+ // modbus_reset();
+ hal.spindle.set_state(hal.spindle.get_state(), sys.spindle_rpm);
+ break;
+
+ case VFD_GetRPM:
+ // modbus_reset();
+ hal.spindle.get_state();
+ break;
+
+ default:
+ break;
+ }//close switch statement
+ }
+ else{
+ retry_counter = 0;
+ system_raise_alarm(Alarm_Spindle);
+ }
+}
+
+void yl620_onReportOptions (bool newopt)
+{
+ on_report_options(newopt);
+
+ if(!newopt) {
+ hal.stream.write("[PLUGIN:Yalang VFD YL620A v0.01]" ASCII_EOL);
+ }
+}
+
+void yl620_reset (void)
+{
+ driver_reset();
+}
+
+bool yl620_spindle_select (spindle_id_t spindle_id)
+{
+ if(spindle_id == vfd_spindle_id) {
+
+ if(settings.spindle.ppr == 0)
+ hal.spindle.get_data = spindleGetData;
+
+ } else if(hal.spindle.get_data == spindleGetData)
+ hal.spindle.get_data = NULL;
+
+ if(on_spindle_select)
+ on_spindle_select(spindle_id);
+
+ return true;
+}
+
+void vfd_yl620_init (void)
+{
+ static const vfd_spindle_ptrs_t spindle = {
+ .spindle.cap.variable = On,
+ .spindle.cap.at_speed = On,
+ .spindle.cap.direction = On,
+ .spindle.config = yl620_spindle_config,
+ .spindle.set_state = yl620_spindleSetState,
+ .spindle.get_state = yl620_spindleGetState,
+ .spindle.update_rpm = yl620_spindleUpdateRPM
+ };
+
+ if((vfd_spindle_id = vfd_register(&spindle, "Yalang YS620")) != -1) {
+
+ on_spindle_select = grbl.on_spindle_select;
+ grbl.on_spindle_select = yl620_spindle_select;
+
+ on_report_options = grbl.on_report_options;
+ grbl.on_report_options = yl620_onReportOptions;
+
+ //driver_reset = hal.driver_reset;
+ //hal.driver_reset = yl620_reset;
+ }
+}
+
+#endif