Mirrors_Framework/grblHAL
1
0
Fork 0
mirror of https://github.com/grblHAL/core.git synced 2026-02-05 08:34:01 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
master
grblHAL/grbllib.c
Terje Io 64c35930c7 Refactored offset handling, for improved readability and in preparation for rotation support. 
Added experimental support for G66 (modal macro call) and G67 (end modal macro call).
Made axis letter to axis/motor assignment for axes ABCUVW freely changeable at compile time.
Fix for some G65 arguments being incorrectly validated for normal use (sign, range).
Added repeat support to G65 macro call via the optional L parameter word.
Changed default setting for ABC-axes to rotary.
Changed defaults for jerk settings to 10x acceleration settings.
Disabled jerk for jog, probe and spindle synchronized motion.
Added _active_probe system parameter, returns -1 if no probe inputs available.
Minor bug fix, G5.1 and G33.1 motion commands were not coverted to the correct string equivalent in $G output.
2026-01-25 07:51:44 +01:00

824 lines
22 KiB
C
Raw Permalink Blame History

/*
grbllib.c - An embedded CNC Controller with rs274/ngc (g-code) support
Part of grblHAL
Copyright (c) 2017-2026 Terje Io
Copyright (c) 2011-2015 Sungeun K. Jeon
Copyright (c) 2009-2011 Simen Svale Skogsrud
grblHAL 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.
grblHAL 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 grblHAL. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include "hal.h"
#include "nuts_bolts.h"
#include "tool_change.h"
#include "override.h"
#include "protocol.h"
#include "machine_limits.h"
#include "report.h"
#include "state_machine.h"
#include "nvs_buffer.h"
#include "stream.h"
#if NGC_EXPRESSIONS_ENABLE
#include "ngc_expr.h"
#endif
#if ENABLE_BACKLASH_COMPENSATION
#include "motion_control.h"
#endif
#ifdef KINEMATICS_API
#include "kinematics.h"
#endif
#if COREXY
#include "kinematics/corexy.h"
#endif
#if WALL_PLOTTER
#include "kinematics/wall_plotter.h"
#endif
#if DELTA_ROBOT
#include "kinematics/delta.h"
#endif
#if POLAR_ROBOT
#include "kinematics/polar.h"
#endif
static void task_execute (sys_state_t state);
typedef union {
uint8_t ok;
struct {
uint8_t init :1,
setup :1,
spindle :1,
amass :1,
pulse_delay :1,
unused :3;
};
} driver_startup_t;
#ifndef CORE_TASK_POOL_SIZE
#define CORE_TASK_POOL_SIZE 40
#endif
typedef struct core_task {
uint32_t time;
foreground_task_ptr fn;
void *data;
struct core_task *next;
} core_task_t;
DCRAM system_t sys; //!< System global variable structure.
DCRAM grbl_t grbl;
DCRAM grbl_hal_t hal;
DCRAM static core_task_t task_pool[CORE_TASK_POOL_SIZE];
static driver_startup_t driver = { .ok = 0xFF };
static core_task_t *next_task = NULL, *immediate_task = NULL, *on_booted = NULL, *systick_task = NULL, *last_freed = NULL;
static on_linestate_changed_ptr on_linestate_changed;
static settings_changed_ptr hal_settings_changed;
static stepper_enable_ptr stepper_enable;
#ifdef KINEMATICS_API
kinematics_t kinematics;
#endif
__attribute__((weak)) void board_ports_init (void)
{
// NOOP
}
__attribute__((always_inline)) static inline void task_free (core_task_t *task)
{
task->fn = NULL;
task->next = NULL;
if(last_freed == NULL)
last_freed = task;
}
__attribute__((always_inline)) static inline core_task_t *task_run (core_task_t *task)
{
core_task_t *t = task;
foreground_task_ptr fn = task->fn;
void *data = task->data;
task = task->next;
task_free(t);
fn(data);
return task;
}
void dummy_bool_handler (bool arg)
{
// NOOP
}
void reset_handler (void)
{
report_init_fns();
grbl.on_macro_return = NULL;
}
static bool dummy_irq_claim (irq_type_t irq, uint_fast8_t id, irq_callback_ptr callback)
{
return false;
}
static void report_driver_error (void *data)
{
char msg[40];
driver.ok = ~driver.ok;
strcpy(msg, "Fatal: Incompatible driver (");
strcat(msg, uitoa(driver.ok));
strcat(msg, ")");
report_message(msg, Message_Plain);
}
static void auto_realtime_report (void *data);
static void realtime_report_check (void *data)
{
task_add_delayed(sys.flags.auto_reporting ? auto_realtime_report : realtime_report_check, NULL, settings.report_interval);
}
static void auto_realtime_report (void *data)
{
if(sys.flags.auto_reporting) {
system_set_exec_state_flag(EXEC_STATUS_REPORT);
task_add_delayed(auto_realtime_report, NULL, settings.report_interval);
} else if(settings.report_interval)
task_add_delayed(realtime_report_check, NULL, settings.report_interval);
}
// "Wire" homing signals to limit signals, used when max limit inputs not available.
ISR_CODE static home_signals_t ISR_FUNC(get_homing_status)(void)
{
home_signals_t home;
limit_signals_t limits = hal.limits.get_state();
home.a.value = limits.min.value;
home.b.value = limits.min2.value;
return home;
}
// "Wire" homing signals to limit signals, used when max limit inputs available.
ISR_CODE static home_signals_t ISR_FUNC(get_homing_status2)(void)
{
home_signals_t home;
limit_signals_t source = xbar_get_homing_source(), limits = hal.limits.get_state();
home.a.value = (limits.min.value & source.min.mask) | (limits.max.value & source.max.mask);
home.b.value = (limits.min2.value & source.min2.mask) | (limits.max2.value & source.max2.mask);
return home;
}
static void output_welcome_message (void *data)
{
grbl.report.init_message(hal.stream.write);
}
static void onLinestateChanged (serial_linestate_t state)
{
if(state.dtr) {
task_delete(output_welcome_message, NULL);
task_add_delayed(output_welcome_message, NULL, 200);
}
if(on_linestate_changed)
on_linestate_changed(state);
}
static void stepperEnable (axes_signals_t enable, bool hold)
{
if(stepper_enable)
stepper_enable(enable, hold);
sys.steppers_enabled = /*!hold &&*/ enable.bits == AXES_BITMASK;
}
static void print_pos_msg (void *data)
{
hal.stream.write("grblHAL: power on self-test (POS) failed!" ASCII_EOL);
if(on_booted) do {
} while((on_booted = task_run(on_booted)));
}
static void onPosFailure (serial_linestate_t state)
{
if(state.dtr) // delay a bit to let the USB stack come up
task_add_delayed(print_pos_msg, NULL, 50);
}
static bool onProbeToolsetter (tool_data_t *tool, coord_data_t *position, bool at_g59_3, bool on)
{
bool ok = false;
if(at_g59_3 && settings.probe.toolsetter_auto_select)
ok = hal.probe.select(on ? Probe_Toolsetter : Probe_Default);
return ok;
}
static void tool_changed (tool_data_t *tool)
{
if(settings.flags.tool_persistent && tool->tool_id != settings.tool_id) {
settings.tool_id = tool->tool_id;
settings_write_global();
}
}
static void settings_changed (settings_t *settings, settings_changed_flags_t changed)
{
hal_settings_changed(settings, changed);
if(grbl.on_settings_changed)
grbl.on_settings_changed(settings, changed);
}
static atc_status_t atc_get_state (void)
{
return hal.driver_cap.atc ? ATC_Online : ATC_None;
}
// main entry point
int grbl_enter (void)
{
assert(NVS_ADDR_PARAMETERS + N_CoordinateSystems * (sizeof(coord_data_t) + NVS_CRC_BYTES) < NVS_ADDR_STARTUP_BLOCK);
assert(NVS_ADDR_STARTUP_BLOCK + N_STARTUP_LINE * (sizeof(stored_line_t) + NVS_CRC_BYTES) < NVS_ADDR_BUILD_INFO);
bool looping = true;
memset(&sys, 0, sizeof(system_t));
memset(&task_pool, 0, sizeof(task_pool));
// Clear all and set some core function pointers
memset(&grbl, 0, sizeof(grbl_t));
grbl.on_execute_realtime = grbl.on_execute_delay = task_execute;
grbl.enqueue_gcode = protocol_enqueue_gcode;
grbl.enqueue_realtime_command = stream_enqueue_realtime_command;
grbl.on_report_options = dummy_bool_handler;
grbl.on_report_command_help = system_command_help;
grbl.on_get_alarms = alarms_get_details;
grbl.on_get_errors = errors_get_details;
grbl.on_get_settings = settings_get_details;
grbl.on_tool_changed = tool_changed;
#if NGC_EXPRESSIONS_ENABLE
grbl.on_process_gcode_comment = ngc_process_comment;
#endif
// Clear all and set some HAL function pointers
memset(&hal, 0, sizeof(grbl_hal_t));
hal.version = HAL_VERSION; // Update when signatures and/or contract is changed - driver_init() should fail
hal.driver_reset = reset_handler;
hal.irq_enable = dummy_handler;
hal.irq_disable = dummy_handler;
hal.irq_claim = dummy_irq_claim;
hal.nvs.size = GRBL_NVS_SIZE;
hal.step_us_min = 2.0f;
hal.coolant_cap.flood = On;
hal.limits.interrupt_callback = limit_interrupt_handler;
hal.control.interrupt_callback = control_interrupt_handler;
hal.stepper.interrupt_callback = stepper_driver_interrupt_handler;
hal.stream_blocking_callback = stream_tx_blocking;
hal.tool.atc_get_state = atc_get_state;
hal.signals_pullup_disable_cap.value = (uint16_t)-1;
sys.cold_start = true;
limits_init();
settings_clear();
report_init_fns();
#ifdef KINEMATICS_API
memset(&kinematics, 0, sizeof(kinematics_t));
#endif
driver.init = driver_init();
#if NVSDATA_BUFFER_ENABLE
nvs_buffer_alloc(); // Allocate memory block for NVS buffer
#endif
#ifdef DEBUGOUT
debug_stream_init();
#endif
#if COMPATIBILITY_LEVEL > 0
hal.stream.suspend_read = NULL;
#endif
#ifdef NO_SAFETY_DOOR_SUPPORT
hal.signals_cap.safety_door_ajar = Off;
#endif
#if COREXY
corexy_init();
#endif
#if WALL_PLOTTER
wall_plotter_init();
#endif
#if DELTA_ROBOT
delta_robot_init();
#endif
#if POLAR_ROBOT
polar_init();
#endif
#if NVSDATA_BUFFER_ENABLE
nvs_buffer_init();
#endif
settings_init(); // Load settings from non-volatile storage
memset(sys.position, 0, sizeof(sys.position)); // Clear machine position.
// check and configure driver
stepper_enable = hal.stepper.enable;
hal.stepper.enable = stepperEnable;
#if ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
driver.amass = hal.driver_cap.amass_level >= MAX_AMASS_LEVEL;
hal.driver_cap.amass_level = MAX_AMASS_LEVEL;
#else
hal.driver_cap.amass_level = 0;
#endif
#ifdef DEFAULT_STEP_PULSE_DELAY
driver.pulse_delay = hal.driver_cap.step_pulse_delay;
#endif
/*
#if AXIS_N_SETTINGS > 4
driver_ok = driver_ok & hal.driver_cap.axes >= AXIS_N_SETTINGS;
#endif
*/
sys.mpg_mode = false;
if((sys.ioinit_pending = driver.ok == 0xFF)) {
hal_settings_changed = hal.settings_changed;
hal.settings_changed = settings_changed;
driver.setup = hal.driver_setup(&settings);
sys.ioinit_pending = false;
}
spindle_id_t spindle_id, encoder_spindle;
// Sanity checks
if(!spindle_get_id(settings.spindle.ref_id, &spindle_id)) {
spindle_ptrs_t *spindle = spindle_get_hal(spindle_get_default(), SpindleHAL_Raw);
spindle_id = spindle ? spindle->id : 0;
settings.spindle.ref_id = spindle ? spindle->ref_id : DEFAULT_SPINDLE;
}
if(!spindle_get_id(settings.spindle.encoder_spindle, &encoder_spindle))
settings.spindle.encoder_spindle = settings.spindle.ref_id;
//
if((driver.spindle = spindle_select(spindle_id))) {
spindle_ptrs_t *spindle = spindle_get(0);
driver.spindle = spindle->get_pwm == NULL || spindle->update_pwm != NULL;
} else
driver.spindle = spindle_select(spindle_add_null());
if(!hal.driver_cap.sd_card)
settings.macro_atc_flags.error_on_no_macro = Off;
if(driver.ok != 0xFF) {
sys.alarm = Alarm_SelftestFailed;
task_run_on_startup(report_driver_error, NULL);
}
hal.stepper.enable(settings.steppers.energize, true);
spindle_all_off();
hal.coolant.set_state((coolant_state_t){0});
if(hal.get_position)
hal.get_position(&sys.position); // TODO: restore on abort when returns true?
#if ENABLE_BACKLASH_COMPENSATION
mc_backlash_init((axes_signals_t){AXES_BITMASK});
#endif
sys.driver_started = sys.alarm != Alarm_SelftestFailed;
// "Wire" homing switches to limit switches if not provided by the driver.
if(hal.homing.get_state == NULL || settings.homing.flags.use_limit_switches)
hal.homing.get_state = hal.limits_cap.max.mask ? get_homing_status2 : get_homing_status;
if(settings.report_interval)
task_add_delayed(auto_realtime_report, NULL, settings.report_interval);
if(hal.driver_cap.sd_card || hal.driver_cap.littlefs) {
fs_options_t fs_options = { .hierarchical_listing = On };
fs_options.lfs_hidden = hal.driver_cap.littlefs;
fs_options.sd_mount_on_boot = hal.driver_cap.sd_card;
setting_remove_elements(Setting_FSOptions, fs_options.mask, true);
}
if(hal.stream.state.linestate_event && !hal.stream.state.passthru) {
on_linestate_changed = hal.stream.on_linestate_changed;
hal.stream.on_linestate_changed = onLinestateChanged;
}
if(grbl.on_probe_toolsetter == NULL && hal.driver_cap.toolsetter && hal.probe.select)
grbl.on_probe_toolsetter = onProbeToolsetter;
if(hal.driver_cap.probe && hal.signals_cap.probe_disconnected)
task_run_on_startup(probe_connected_event, hal.control.get_state().probe_disconnected ? NULL : (void *)1);
// Initialization loop upon power-up or a system abort. For the latter, all processes
// will return to this loop to be cleanly re-initialized.
while(looping) {
spindle_num_t spindle_num = N_SYS_SPINDLE;
// Reset report entry points
report_init_fns();
overrides_t override;
memcpy(&override, &sys.override, sizeof(overrides_t));
if(!sys.position_lost || settings.homing.flags.keep_on_reset)
memset(&sys, 0, offsetof(system_t, homed)); // Clear system variables except alarm & homed status.
else
memset(&sys, 0, offsetof(system_t, alarm)); // Clear system variables except state & alarm.
sys.var5399 = -2; // Clear last M66 result
sys.override.feed_rate = sys.cold_start || !settings.flags.keep_feed_override_on_reset ? DEFAULT_FEED_OVERRIDE : override.feed_rate;
sys.override.rapid_rate = sys.cold_start || !settings.flags.keep_rapids_override_on_reset ? DEFAULT_RAPID_OVERRIDE : override.rapid_rate;
do {
if(spindle_is_enabled(--spindle_num))
spindle_get(spindle_num)->param->override_pct = DEFAULT_SPINDLE_RPM_OVERRIDE; // Set to 100%
} while(spindle_num);
sys.flags.auto_reporting = settings.report_interval != 0;
if(settings.parking.flags.enabled)
sys.override.control.parking_disable = settings.parking.flags.deactivate_upon_init;
flush_override_buffers();
// Reset primary systems.
hal.stream.reset_read_buffer(); // Clear input stream buffer
gc_init(settings.flags.keep_offsets_on_reset); // Set g-code parser to default state
hal.limits.enable(settings.limits.flags.hard_enabled, (axes_signals_t){0});
plan_reset(); // Clear block buffer and planner variables
st_reset(); // Clear stepper subsystem variables.
limits_set_homing_axes(); // Set axes to be homed from settings.
system_init_switches(); // Set switches from inputs.
// Sync cleared gcode and planner positions to current system position.
sync_position();
if(hal.stepper.disable_motors)
hal.stepper.disable_motors((axes_signals_t){0}, SquaringMode_Both);
if(!hal.driver_cap.atc)
tc_init();
// Print welcome message. Indicates an initialization has occurred at power-up or with a reset.
grbl.report.init_message(hal.stream.write_all);
if(!settings.flags.no_unlock_after_estop && state_get() == STATE_ESTOP)
state_set(STATE_ALARM);
if(hal.driver_cap.mpg_mode)
protocol_enqueue_realtime_command(sys.mpg_mode ? CMD_STATUS_REPORT_ALL : CMD_STATUS_REPORT);
// Start main loop. Processes program inputs and executes them.
if(!(looping = protocol_main_loop()))
looping = hal.driver_release == NULL || hal.driver_release();
sys.cold_start = false;
}
nvs_buffer_free();
return 0;
}
__attribute__((always_inline)) static inline core_task_t *task_alloc (void)
{
core_task_t *task = NULL;
uint_fast8_t idx = CORE_TASK_POOL_SIZE;
if(last_freed) {
task = last_freed;
last_freed = NULL;
} else do {
if(task_pool[--idx].fn == NULL)
task = &task_pool[idx];
} while(task == NULL && idx);
return task;
}
static void task_execute (sys_state_t state)
{
static uint32_t last_ms = 0;
core_task_t *task;
if(immediate_task && sys.driver_started) {
hal.irq_disable();
if((task = immediate_task))
immediate_task = NULL;
hal.irq_enable();
if(task) do {
} while((task = task_run(task)));
}
uint32_t now = hal.get_elapsed_ticks();
if(now == last_ms || next_task == systick_task)
return;
last_ms = now;
if((task = systick_task)) do {
task->fn(task->data);
} while((task = task->next));
while((task = next_task) && (int32_t)(task->time - now) <= 0) {
hal.irq_disable();
if(task == next_task)
next_task = task->next;
else {
core_task_t *t;
if((t = next_task)) {
while(t->next && t->next != task)
t = t->next;
if(t->next && t->next == task)
t->next = task->next;
}
}
hal.irq_enable();
void *data = task->data;
foreground_task_ptr fn = task->fn;
task_free(task);
fn(data);
}
}
ISR_CODE bool ISR_FUNC(task_add_delayed)(foreground_task_ptr fn, void *data, uint32_t delay_ms)
{
core_task_t *task = NULL;
hal.irq_disable();
if(fn && (task = task_alloc())) {
task->time = hal.get_elapsed_ticks() + delay_ms;
task->fn = fn;
task->data = data;
task->next = NULL;
if(next_task == NULL)
next_task = task;
else if((int32_t)(task->time - next_task->time) <= 0) {
task->next = next_task;
next_task = task;
} else {
core_task_t *t = next_task;
while(t) {
if(t->next == NULL || (int32_t)(task->time - t->next->time) < 0) {
task->next = t->next;
t->next = task;
break;
}
t = t->next;
}
}
}
hal.irq_enable();
return task != NULL;
}
ISR_CODE void task_delete (foreground_task_ptr fn, void *data)
{
core_task_t *task, *prev = NULL;
hal.irq_disable();
if((task = next_task)) do {
if(fn == task->fn && (data == NULL || data == task->data)) {
if(prev)
prev->next = task->next;
else
next_task = task->next;
task_free(task);
break;
}
prev = task;
} while((task = task->next));
hal.irq_enable();
}
ISR_CODE bool ISR_FUNC(task_add_systick)(foreground_task_ptr fn, void *data)
{
core_task_t *task = NULL;
hal.irq_disable();
if(fn && (task = task_alloc())) {
task->fn = fn;
task->data = data;
task->next = NULL;
if(systick_task == NULL)
systick_task = task;
else {
core_task_t *t = systick_task;
while(t->next)
t = t->next;
t->next = task;
}
}
hal.irq_enable();
return task != NULL;
}
void task_delete_systick (foreground_task_ptr fn, void *data)
{
core_task_t *task, *prev = NULL;
hal.irq_disable();
if((task = systick_task)) do {
if(fn == task->fn && data == task->data) {
if(prev)
prev->next = task->next;
else
systick_task = task->next;
task_free(task);
break;
}
prev = task;
} while((task = task->next));
hal.irq_enable();
}
/*! \brief Enqueue a function to be called once by the foreground process.
\param fn pointer to a \a foreground_task_ptr type of function.
\param data pointer to data to be passed to the callee.
\returns true if successful, false otherwise.
*/
ISR_CODE bool ISR_FUNC(task_add_immediate)(foreground_task_ptr fn, void *data)
{
core_task_t *task = NULL;
hal.irq_disable();
if(fn && (task = task_alloc())) {
task->fn = fn;
task->data = data;
task->next = NULL;
if(immediate_task == NULL)
immediate_task = task;
else {
core_task_t *t = immediate_task;
while(t->next)
t = t->next;
t->next = task;
}
}
hal.irq_enable();
return task != NULL;
}
/*! \brief Enqueue a function to be called once by the foreground process after the boot sequence is completed.
\param fn pointer to a \a foreground_task_ptr type of function.
\param data pointer to data to be passed to the callee.
\returns true if successful, false otherwise.
*/
ISR_CODE bool ISR_FUNC(task_run_on_startup)(foreground_task_ptr fn, void *data)
{
if(sys.cold_start) {
core_task_t *task = NULL;
hal.irq_disable();
if(fn && (task = task_alloc())) {
task->fn = fn;
task->data = data;
task->next = NULL;
if(on_booted == NULL)
on_booted = task;
else {
core_task_t *t = on_booted;
while(t->next)
t = t->next;
t->next = task;
}
}
hal.irq_enable();
return task != NULL;
} else
return task_add_immediate(fn, data); // TODO: for now, to be removed...
}
// for core use only, called once from protocol.c on cold start
void task_execute_on_startup (void)
{
if(!sys.driver_started) {
// Clear task queues except startup warnings.
core_task_t *task, *prev = NULL;
if((task = on_booted)) do {
if(!(task->fn == report_warning)) {
if(prev)
prev->next = task->next;
else {
prev = NULL;
on_booted = task->next;
}
task_free(task);
} else
prev = task;
} while((task = prev ? prev->next : on_booted));
while(next_task)
task_delete(next_task->fn, NULL);
while(systick_task)
task_delete_systick(systick_task->fn, NULL);
}
if(on_booted && (sys.driver_started || !hal.stream.state.linestate_event)) do {
} while((on_booted = task_run(on_booted)));
if(!sys.driver_started) {
if(hal.stream.state.linestate_event)
hal.stream.on_linestate_changed = onPosFailure;
while(true)
grbl.on_execute_realtime(state_get());
}
}
void task_raise_alarm (void *data)
{
system_raise_alarm((alarm_code_t)data);
}
Reference in New Issue View Git Blame Copy Permalink