Changed kinematics API and implementations (corexy and wallplotter) to allow backlash compensation. Ref [ESP32 issue 25](https://github.com/grblHAL/ESP32/issues/25).

Fixed feed rate handling for corexy kinematics. Ref issue #147.
Fixed tool table/tool change bugs. Ref. [ioSender issue 228](https://github.com/terjeio/ioSender/issues/228).

README.md

@@ -11,7 +11,7 @@ It has been written to complement grblHAL and has features such as proper keyboa
 
 ---
 
-Latest build date is 20220325, see the [changelog](changelog.md) for details.  
+Latest build date is 20220612, see the [changelog](changelog.md) for details.  
 __NOTE:__ A settings reset will be performed on an update for versions earlier than 20211122. Backup and restore of settings is recommended.  
 __IMPORTANT!__  A new setting has been introduced for ganged axes motors in version 20211121.  
 I have only bench tested this for a couple of drivers, correct function should be verified after updating by those who have more than three motors configured.  

alarms.c

@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2017-2021 Terje Io
+  Copyright (c) 2017-2022 Terje Io
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
   Copyright (c) 2009-2011 Simen Svale Skogsrud
 
@@ -43,7 +43,8 @@ PROGMEM static const alarm_detail_t alarm_detail[] = {
     { Alarm_Spindle, "Spindle at speed timeout. Clear before continuing." },
     { Alarm_HomingFailAutoSquaringApproach, "Homing fail. Could not find second limit switch for auto squared axis within search distances. Try increasing max travel, decreasing pull-off distance, or check wiring." },
     { Alarm_SelftestFailed, "Power on selftest (POS) failed." },
-    { Alarm_MotorFault, "Motor fault." }
+    { Alarm_MotorFault, "Motor fault." },
+    { Alarm_HomingFail, "Homing fail. Bad configuration." }
 };
 
 static alarm_details_t details = {

alarms.h

@@ -44,7 +44,8 @@ typedef enum {
     Alarm_HomingFailAutoSquaringApproach = 15,
     Alarm_SelftestFailed = 16,
     Alarm_MotorFault = 17,
-    Alarm_AlarmMax = Alarm_MotorFault
+    Alarm_HomingFail = 18,
+    Alarm_AlarmMax = Alarm_HomingFail
 } alarm_code_t;
 
 typedef struct {

changelog.md

@@ -1,5 +1,25 @@
 ## grblHAL changelog
 
+20220612:
+
+Core:
+
+* Changed kinematics API and implementations (corexy and wallplotter) to allow backlash compensation. Ref [ESP32 issue 25](https://github.com/grblHAL/ESP32/issues/25).
+
+* Fixed feed rate handling for corexy kinematics. Ref issue #147.
+
+* Fixed tool table/tool change bugs. Ref. [ioSender issue 228](https://github.com/terjeio/ioSender/issues/228).
+
+Drivers:
+
+* iMXRT1062 : Fix for issue #38.
+
+* STM32F4xx : Added missing code guard for boards not having a spindle direction pin. Improved UART channel assignment handling.
+
+* STM32F7xx : Improved UART channel assignment handling.
+
+* ESP32 : Added missing file \(corexy.c\) to filelist, fixed incorrect URL in readme.
+
 20220416:
 
 Drivers:

config.h

@@ -74,7 +74,7 @@ __NOTE:__ if switching to a level > 1 please reset non-volatile storage with \a
 
 // Enable CoreXY kinematics. Use ONLY with CoreXY machines.
 // IMPORTANT: If homing is enabled, you must reconfigure the homing cycle #defines above to
-// #define HOMING_CYCLE_0 X_AXIS_BIT and #define HOMING_CYCLE_1 Y_AXIS_BIT
+//#define HOMING_CYCLE_0 X_AXIS_BIT and #define HOMING_CYCLE_1 Y_AXIS_BIT
 // NOTE: This configuration option alters the motion of the X and Y axes to principle of operation
 // defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as
 // described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors
@@ -113,8 +113,8 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // ADVANCED CONFIGURATION OPTIONS:
 
 // Enables code for debugging purposes. Not for general use and always in constant flux.
-// #define DEBUG // Uncomment to enable. Default disabled.
-// #define DEBUGOUT 0 // Uncomment to claim serial port with given instance number and add HAL entry point for debug output.
+//#define DEBUG // Uncomment to enable. Default disabled.
+//#define DEBUGOUT 0 // Uncomment to claim serial port with given instance number and add HAL entry point for debug output.
 
 // Some status report data isn't necessary for realtime, only intermittently, because the values don't
 // change often. The following macros configures how many times a status report needs to be called before
@@ -172,7 +172,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // value. This also ensures that a planned motion always completes and accounts for any floating-point
 // round-off errors. Although not recommended, a lower value than 1.0 mm/min will likely work in smaller
 // machines, perhaps to 0.1mm/min, but your success may vary based on multiple factors.
-// #define MINIMUM_FEED_RATE 1.0f // (mm/min)
+//#define MINIMUM_FEED_RATE 1.0f // (mm/min)
 
 // Number of arc generation iterations by small angle approximation before exact arc trajectory
 // correction with expensive sin() and cos() calculations. This parameter maybe decreased if there
@@ -208,7 +208,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // available RAM, like when re-compiling for MCU with ample amounts of RAM. Or decrease if the MCU begins to
 // crash due to the lack of available RAM or if the CPU is having trouble keeping up with planning
 // new incoming motions as they are executed.
-// #define BLOCK_BUFFER_SIZE 36 // Uncomment to override default in planner.h.
+//#define BLOCK_BUFFER_SIZE 36 // Uncomment to override default in planner.h.
 
 // Governs the size of the intermediary step segment buffer between the step execution algorithm
 // and the planner blocks. Each segment is set of steps executed at a constant velocity over a
@@ -216,11 +216,11 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // block velocity profile is traced exactly. The size of this buffer governs how much step
 // execution lead time there is for other Grbl processes have to compute and do their thing
 // before having to come back and refill this buffer, currently at ~50msec of step moves.
-// #define SEGMENT_BUFFER_SIZE 10 // Uncomment to override default in stepper.h.
+//#define SEGMENT_BUFFER_SIZE 10 // Uncomment to override default in stepper.h.
 
 // Configures the position after a probing cycle during Grbl's check mode. Disabled sets
 // the position to the probe target, when enabled sets the position to the start position.
-// #define SET_CHECK_MODE_PROBE_TO_START // Default disabled. Uncomment to enable.
+//#define SET_CHECK_MODE_PROBE_TO_START // Default disabled. Uncomment to enable.
 
 // Force Grbl to check the state of the hard limit switches when the processor detects a pin
 // change inside the hard limit ISR routine. By default, Grbl will trigger the hard limits
@@ -230,7 +230,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // that the switches don't bounce, we recommend enabling this option. This will help prevent
 // triggering a hard limit when the machine disengages from the switch.
 // NOTE: This option has no effect if SOFTWARE_DEBOUNCE is enabled.
-// #define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable.
+//#define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable.
 
 // Adjusts homing cycle search and locate scalars. These are the multipliers used by Grbl's
 // homing cycle to ensure the limit switches are engaged and cleared through each phase of
@@ -239,8 +239,8 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // uses the homing pull-off distance setting times the LOCATE_SCALAR to pull-off and re-engage
 // the limit switch.
 // NOTE: Both of these values must be greater than 1.0 to ensure proper function.
-// #define HOMING_AXIS_SEARCH_SCALAR  1.5f // Uncomment to override defaults in limits.c.
-// #define HOMING_AXIS_LOCATE_SCALAR  10.0f // Uncomment to override defaults in limits.c.
+//#define HOMING_AXIS_SEARCH_SCALAR  1.5f // Uncomment to override defaults in limits.c.
+//#define HOMING_AXIS_LOCATE_SCALAR  10.0f // Uncomment to override defaults in limits.c.
 
 // Enable the '$RST=*', '$RST=$', and '$RST=#' non-volatile storage restore commands. There are cases where
 // these commands may be undesirable. Simply comment the desired macro to disable it.
@@ -268,7 +268,7 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // NOTE: If disabled and to ensure Grbl can never alter the build info line, you'll also need to enable
 // the SETTING_RESTORE_ALL macro above and remove SETTINGS_RESTORE_BUILD_INFO from the mask.
 // NOTE: See the included grblWrite_BuildInfo.ino example file to write this string seperately.
-// #define DISABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Uncomment to disable.
+//#define DISABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Uncomment to disable.
 
 // Enables and configures Grbl's sleep mode feature. If the spindle or coolant are powered and Grbl
 // is not actively moving or receiving any commands, a sleep timer will start. If any data or commands
@@ -372,11 +372,11 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // instead of ground.
 // WARNING: When the pull-ups are disabled, this might require additional wiring with pull-down resistors!
 //          Please check driver code and/or documentation.
-// #define DISABLE_LIMIT_BITS_PULL_UP_MASK AXES_BITMASK
-// #define DISABLE_LIMIT_BITS_PULL_UP_MASK (X_AXIS_BIT|Y_AXIS_BIT)
-// #define DISABLE_CONTROL_PINS_PULL_UP_MASK SIGNALS_BITMASK
-// #define DISABLE_CONTROL_PINS_PULL_UP_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT)
-// #define DISABLE_PROBE_BIT_PULL_UP
+//#define DISABLE_LIMIT_BITS_PULL_UP_MASK AXES_BITMASK
+//#define DISABLE_LIMIT_BITS_PULL_UP_MASK (X_AXIS_BIT|Y_AXIS_BIT)
+//#define DISABLE_CONTROL_PINS_PULL_UP_MASK SIGNALS_BITMASK
+//#define DISABLE_CONTROL_PINS_PULL_UP_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT)
+//#define DISABLE_PROBE_BIT_PULL_UP
 
 // If your machine has two limits switches wired in parallel to one axis, you will need to enable
 // this feature. Since the two switches are sharing a single pin, there is no way for Grbl to tell
@@ -394,9 +394,13 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 //#define ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES // Default disabled. Uncomment to enable.
 
 // Inverts logic of the stepper enable signal(s).
+#if COMPATIBILITY_LEVEL <= 2
 // NOTE: Not universally available for individual axes - check driver documentation.
 //       Specify at least X_AXIS_BIT if a common enable signal is used.
-// #define INVERT_ST_ENABLE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable.
+//#define INVERT_ST_ENABLE_MASK (X_AXIS_BIT|Y_AXIS_BIT|Z_AXIS_BIT) // Default disabled. Uncomment to enable.
+#else
+//#define INVERT_ST_ENABLE_MASK 1 // Default disabled. Uncomment to enable.
+#endif
 // Mask to be OR'ed with stepper disable signal(s). Axes configured will not be disabled.
 // NOTE: Not universally available for individual axes - check driver documentation.
 //       Specify at least X_AXIS_BIT if a common enable signal is used.
@@ -414,20 +418,20 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 // normally-open (NO) switches on the specified pins, rather than the default normally-closed (NC) switches.
 // NOTE: The first option will invert all control pins. The second option is an example of
 // inverting only a few pins. See the start of this file for other signal definitions.
-// #define INVERT_CONTROL_PIN_MASK SIGNALS_BITMASK // Default disabled. Uncomment to enable.
-// #define INVERT_CONTROL_PIN_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) // Default disabled. Uncomment to enable.
-// #define INVERT_LIMIT_BIT_MASK AXES_BITMASK // Default disabled. Uncomment to enable. Uncomment to enable.
-// #define INVERT_LIMIT_BIT_MASK (X_AXIS_BIT|Y_AXIS_BIT) // Default disabled. Uncomment to enable.
+//#define INVERT_CONTROL_PIN_MASK SIGNALS_BITMASK // Default disabled. Uncomment to enable.
+//#define INVERT_CONTROL_PIN_MASK (SIGNALS_SAFETYDOOR_BIT|SIGNALS_RESET_BIT) // Default disabled. Uncomment to enable.
+//#define INVERT_LIMIT_BIT_MASK AXES_BITMASK // Default disabled. Uncomment to enable. Uncomment to enable.
+//#define INVERT_LIMIT_BIT_MASK (X_AXIS_BIT|Y_AXIS_BIT) // Default disabled. Uncomment to enable.
 // For inverting the probe pin use DEFAULT_INVERT_PROBE_BIT in defaults.h
 
 // Inverts the selected spindle output signals from active high to active low. Useful for some pre-built electronic boards.
-// #define INVERT_SPINDLE_ENABLE_PIN 1 // Default disabled. Uncomment to enable.
-// #define INVERT_SPINDLE_CCW_PIN 1    // Default disabled. Uncomment to enable. NOTE: not supported by all drivers.
-// #define INVERT_SPINDLE_PWM_PIN 1    // Default disabled. Uncomment to enable. NOTE: not supported by all drivers.
+//#define INVERT_SPINDLE_ENABLE_PIN 1 // Default disabled. Uncomment to enable.
+//#define INVERT_SPINDLE_CCW_PIN 1    // Default disabled. Uncomment to enable. NOTE: not supported by all drivers.
+//#define INVERT_SPINDLE_PWM_PIN 1    // Default disabled. Uncomment to enable. NOTE: not supported by all drivers.
 
 // Inverts the selected coolant signals from active high to active low. Useful for some pre-built electronic boards.
-// #define INVERT_COOLANT_FLOOD_PIN 1 // Default disabled. Uncomment to enable.
-// #define INVERT_COOLANT_MIST_PIN 1  // Default disabled. Note: not supported by all drivers.
+//#define INVERT_COOLANT_FLOOD_PIN 1 // Default disabled. Uncomment to enable.
+//#define INVERT_COOLANT_MIST_PIN 1  // Default disabled. Note: not supported by all drivers.
 
 
 // Used by variable spindle output only. This forces the PWM output to a minimum duty cycle when enabled.

core_handlers.h

@@ -84,6 +84,7 @@ typedef bool (*on_probe_fixture_ptr)(tool_data_t *tool, bool at_g59_3, bool on);
 typedef bool (*on_probe_start_ptr)(axes_signals_t axes, float *target, plan_line_data_t *pl_data);
 typedef void (*on_probe_completed_ptr)(void);
 typedef void (*on_toolchange_ack_ptr)(void);
+typedef void (*on_jog_cancel_ptr)(sys_state_t state);
 typedef bool (*on_spindle_select_ptr)(spindle_id_t spindle_id);
 typedef status_code_t (*on_unknown_sys_command_ptr)(sys_state_t state, char *line); // return Status_Unhandled.
 typedef status_code_t (*on_user_command_ptr)(char *line);
@@ -116,6 +117,7 @@ typedef struct {
     on_probe_start_ptr on_probe_start;
     on_probe_completed_ptr on_probe_completed;
     on_toolchange_ack_ptr on_toolchange_ack; // Called from interrupt context.
+    on_jog_cancel_ptr on_jog_cancel; // Called from interrupt context.
     on_laser_ppi_enable_ptr on_laser_ppi_enable;
     on_spindle_select_ptr on_spindle_select;
     // core entry points - set up by core before driver_init() is called.

corexy.c

@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2019 Terje Io
+  Copyright (c) 2019-2022 Terje Io
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
 
   Grbl is free software: you can redistribute it and/or modify
@@ -24,6 +24,8 @@
 
 #ifdef COREXY
 
+#include <math.h>
+
 #include "settings.h"
 #include "planner.h"
 #include "kinematics.h"
@@ -45,37 +47,27 @@ inline static int32_t corexy_convert_to_b_motor_steps (int32_t *steps)
 }
 
 // Returns machine position of axis 'idx'. Must be sent a 'step' array.
-static void corexy_convert_array_steps_to_mpos (float *position, int32_t *steps)
+static float *corexy_convert_array_steps_to_mpos (float *position, int32_t *steps)
 {
     position[X_AXIS] = corexy_convert_to_a_motor_steps(steps) / settings.axis[X_AXIS].steps_per_mm;
     position[Y_AXIS] = corexy_convert_to_b_motor_steps(steps) / settings.axis[Y_AXIS].steps_per_mm;
     position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm;
+
+    return position;
 }
 
-// Transform absolute position from cartesian coordinate system (mm) to corexy coordinate system (step)
-static void corexy_target_to_steps (int32_t *target_steps, float *target)
+// Transform position from cartesian coordinate system to corexy coordinate system
+static inline float *transform_from_cartesian (float *target, float *position)
 {
-    uint_fast8_t idx = N_AXIS;
-    int32_t a_steps, b_steps;
+    uint_fast8_t idx;
 
-    do {
-        switch(--idx) {
-            case X_AXIS:
-                a_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
-                break;
+    target[X_AXIS] = position[X_AXIS] + position[Y_AXIS];
+    target[Y_AXIS] = position[X_AXIS] - position[Y_AXIS];
 
-            case Y_AXIS:
-                b_steps = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
-                break;
+    for(idx = Z_AXIS; idx < N_AXIS; idx++)
+        target[idx] = position[idx];
 
-            default:
-                target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
-                break;
-        }
-    } while(idx);
-
-    target_steps[A_MOTOR] = a_steps + b_steps;
-    target_steps[B_MOTOR] = a_steps - b_steps;
+    return target;
 }
 
 static uint_fast8_t corexy_limits_get_axis_mask (uint_fast8_t idx)
@@ -151,6 +143,57 @@ static void corexy_limits_set_machine_positions (axes_signals_t cycle)
     } while(idx);
 }
 
+static inline float get_distance (float *p0, float *p1)
+{
+    uint_fast8_t idx = N_AXIS;
+    float distance = 0.0f;
+
+    do {
+        idx--;
+        distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
+    } while(idx);
+
+    return sqrtf(distance);
+}
+
+// called from mc_line() to segment lines if not overridden, default implementation for pass-through
+static float *kinematics_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init)
+{
+    static uint_fast8_t iterations;
+    static float trsf[N_AXIS];
+
+    if(init) {
+
+        iterations = 2;
+
+        transform_from_cartesian(trsf, target);
+
+        if(!pl_data->condition.rapid_motion) {
+
+            uint_fast8_t idx;
+            float cpos[N_AXIS];
+
+            cpos[X_AXIS] = (position[X_AXIS] + position[Y_AXIS]) * .5f;
+            cpos[Y_AXIS] = (position[X_AXIS] - position[Y_AXIS]) * .5f;
+            for(idx = Z_AXIS; idx < N_AXIS; idx++)
+                cpos[idx] = position[idx];
+
+            pl_data->feed_rate *= get_distance(trsf, position) / get_distance(target, cpos);
+        }
+    }
+
+    return iterations-- == 0 ? NULL : trsf;
+}
+
+static bool homing_cycle_validate (axes_signals_t cycle)
+{
+    return (cycle.mask & (X_AXIS_BIT|Y_AXIS_BIT)) == 0 || cycle.mask < 3;
+}
+
+static float homing_cycle_get_feedrate (float feedrate, axes_signals_t cycle)
+{
+    return feedrate * sqrtf(2.0f);
+}
 
 // Initialize API pointers for CoreXY kinematics
 void corexy_init (void)
@@ -158,9 +201,11 @@ void corexy_init (void)
     kinematics.limits_set_target_pos = corexy_limits_set_target_pos;
     kinematics.limits_get_axis_mask = corexy_limits_get_axis_mask;
     kinematics.limits_set_machine_positions = corexy_limits_set_machine_positions;
-    kinematics.plan_target_to_steps = corexy_target_to_steps;
-    kinematics.convert_array_steps_to_mpos = corexy_convert_array_steps_to_mpos;
+    kinematics.transform_from_cartesian = transform_from_cartesian;
+    kinematics.transform_steps_to_cartesian = corexy_convert_array_steps_to_mpos;
+    kinematics.segment_line = kinematics_segment_line;
+    kinematics.homing_cycle_validate = homing_cycle_validate;
+    kinematics.homing_cycle_get_feedrate = homing_cycle_get_feedrate;
 }
 
 #endif
-

defaults.h

@@ -573,7 +573,11 @@
 #endif
 
 #ifndef INVERT_ST_ENABLE_MASK
+#if COMPATIBILITY_LEVEL <= 2
 #define INVERT_ST_ENABLE_MASK AXES_BITMASK
+#else
+#define INVERT_ST_ENABLE_MASK 0
+#endif
 #endif
 
 #ifndef INVERT_LIMIT_BIT_MASK

gcode.c

@@ -216,7 +216,9 @@ void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offs
                 idx--;
                 tlo_changed |= gc_state.tool_length_offset[idx] != 0.0f;
                 gc_state.tool_length_offset[idx] = 0.0f;
-                gc_state.tool->offset[idx] = 0;
+#ifndef N_TOOLS
+                gc_state.tool->offset[idx] = 0.0f;
+#endif
             } while(idx);
             break;
 
@@ -225,7 +227,9 @@ void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offs
                 float new_offset = offset / settings.axis[idx].steps_per_mm;
                 tlo_changed |= gc_state.tool_length_offset[idx] != new_offset;
                 gc_state.tool_length_offset[idx] = new_offset;
-                gc_state.tool->offset[idx] = offset;
+#ifndef N_TOOLS
+                gc_state.tool->offset[idx] = new_offset;
+#endif
             }
             break;
 

grbl.h

@@ -34,7 +34,7 @@
 #else
 #define GRBL_VERSION "1.1f"
 #endif
-#define GRBL_BUILD 20220325
+#define GRBL_BUILD 20220612
 
 // The following symbols are set here if not already set by the compiler or in config.h
 // Do NOT change here!

grbllib.c

@@ -70,22 +70,7 @@ grbl_hal_t hal;
 static driver_startup_t driver = { .ok = 0xFF };
 
 #ifdef KINEMATICS_API
-
 kinematics_t kinematics;
-
-// called from mc_line() to segment lines if not overridden, default implementation for pass-through
-static bool kinematics_segment_line (float *target, plan_line_data_t *pl_data, bool init)
-{
-    static uint_fast8_t iterations;
-
-    if(init)
-        iterations = 2;
-    else
-        iterations--;
-
-    return iterations != 0;
-}
-
 #endif
 
 void dummy_bool_handler (bool arg)
@@ -154,8 +139,6 @@ int grbl_enter (void)
 
 #ifdef KINEMATICS_API
     memset(&kinematics, 0, sizeof(kinematics_t));
-
-    kinematics.segment_line = kinematics_segment_line; // default to no segmentation
 #endif
 
     driver.init = driver_init();

kinematics.h

@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2019 Terje Io
+  Copyright (c) 2019-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
@@ -23,12 +23,14 @@
 #define _KINEMATICS_H_
 
 typedef struct {
-    void (*convert_array_steps_to_mpos)(float *position, int32_t *steps);
-    void (*plan_target_to_steps) (int32_t *target_steps, float *target);
-    bool (*segment_line) (float *target, plan_line_data_t *pl_data, bool init);
+    float *(*transform_steps_to_cartesian)(float *position, int32_t *steps);
+    float *(*transform_from_cartesian) (float *target, float *position);
+    float *(*segment_line) (float *target, float *position, plan_line_data_t *pl_data, bool init); // target is cartesian, position transformed
     uint_fast8_t (*limits_get_axis_mask)(uint_fast8_t idx);
     void (*limits_set_target_pos)(uint_fast8_t idx);
     void (*limits_set_machine_positions)(axes_signals_t cycle);
+    bool (*homing_cycle_validate)(axes_signals_t cycle);
+    float (*homing_cycle_get_feedrate)(float feedrate, axes_signals_t cycle);
 } kinematics_t;
 
 extern kinematics_t kinematics;

limits.c

@@ -173,7 +173,12 @@ static bool limits_pull_off (axes_signals_t axis, float distance)
     plan_data.condition.coolant = gc_state.modal.coolant;
     memcpy(&plan_data.spindle, &gc_state.spindle, sizeof(spindle_t));
 
+#ifdef KINEMATICS_API
+    coord_data_t k_target;
+    plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data);    // Bypass mc_line(). Directly plan homing motion.;
+#else
     plan_buffer_line(target.values, &plan_data);    // Bypass mc_line(). Directly plan homing motion.
+#endif
 
     sys.step_control.flags = 0;                 // Clear existing flags and
     sys.step_control.execute_sys_motion = On;   // set to execute homing motion.
@@ -239,7 +244,7 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar
     int32_t initial_trigger_position = 0, autosquare_fail_distance = 0;
     uint_fast8_t n_cycle = (2 * settings.homing.locate_cycles + 1);
     uint_fast8_t step_pin[N_AXIS], n_active_axis, dual_motor_axis = 0;
-    float target[N_AXIS];
+    coord_data_t target;
     float max_travel = 0.0f, homing_rate = settings.homing.seek_rate;
     bool approach = true, autosquare_check = false;
     axes_signals_t axislock, homing_state;
@@ -286,7 +291,7 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar
     do {
 
         // Initialize and declare variables needed for homing routine.
-        system_convert_array_steps_to_mpos(target, sys.position);
+        system_convert_array_steps_to_mpos(target.values, sys.position);
         axislock = (axes_signals_t){0};
         n_active_axis = 0;
 
@@ -303,9 +308,9 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar
 #endif
                 // Set target direction based on cycle mask and homing cycle approach state.
                 if (bit_istrue(settings.homing.dir_mask.value, bit(idx)))
-                    target[idx] = approach ? - max_travel : max_travel;
+                    target.values[idx] = approach ? - max_travel : max_travel;
                 else
-                    target[idx] = approach ? max_travel : - max_travel;
+                    target.values[idx] = approach ? max_travel : - max_travel;
 
                 // Apply axislock to the step port pins active in this cycle.
                 axislock.mask |= step_pin[idx];
@@ -314,6 +319,11 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar
 
         sys.homing_axis_lock.mask = axislock.mask;
 
+#ifdef KINEMATICS_API
+        if(kinematics.homing_cycle_get_feedrate)
+            homing_rate = kinematics.homing_cycle_get_feedrate(homing_rate, cycle);
+#endif
+
         if(grbl.on_homing_rate_set)
             grbl.on_homing_rate_set(cycle, homing_rate, !approach);
 
@@ -321,7 +331,13 @@ static bool limits_homing_cycle (axes_signals_t cycle, axes_signals_t auto_squar
 
         // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
         plan_data.feed_rate = homing_rate;      // Set current homing rate.
-        plan_buffer_line(target, &plan_data);   // Bypass mc_line(). Directly plan homing motion.
+
+#ifdef KINEMATICS_API
+        coord_data_t k_target;
+        plan_buffer_line(kinematics.transform_from_cartesian(k_target.values, target.values), &plan_data);    // Bypass mc_line(). Directly plan homing motion.;
+#else
+        plan_buffer_line(target.values, &plan_data);    // Bypass mc_line(). Directly plan homing motion.
+#endif
 
         sys.step_control.flags = 0;
         sys.step_control.execute_sys_motion = On; // Set to execute homing motion and clear existing flags.

motion_control.c

@@ -94,6 +94,11 @@ void mc_sync_backlash_position (void)
 bool mc_line (float *target, plan_line_data_t *pl_data)
 {
 
+#ifdef KINEMATICS_API
+    float feed_rate = pl_data->feed_rate;
+    target = kinematics.segment_line(target, plan_get_position(), pl_data, true);
+#endif
+
     // If enabled, check for soft limit violations. Placed here all line motions are picked up
     // from everywhere in Grbl.
     // NOTE: Block jog motions. Jogging is a special case and soft limits are handled independently.
@@ -117,6 +122,10 @@ bool mc_line (float *target, plan_line_data_t *pl_data)
         // doesn't update the machine position values. Since the position values used by the g-code
         // parser and planner are separate from the system machine positions, this is doable.
 
+#ifdef KINEMATICS_API
+       while(kinematics.segment_line(target, NULL, pl_data, false)) {
+#endif
+
 #ifdef ENABLE_BACKLASH_COMPENSATION
 
         if(backlash_enabled.mask) {
@@ -169,11 +178,6 @@ bool mc_line (float *target, plan_line_data_t *pl_data)
 
 #endif // Backlash comp
 
-#ifdef KINEMATICS_API
-     kinematics.segment_line(target, pl_data, true);
-
-     while(kinematics.segment_line(target, pl_data, false)) {
-#endif
         // If the buffer is full: good! That means we are well ahead of the robot.
         // Remain in this loop until there is room in the buffer.
          do {
@@ -192,7 +196,18 @@ bool mc_line (float *target, plan_line_data_t *pl_data)
             // Forces a buffer sync while in M3 laser mode only.
             hal.spindle.set_state(pl_data->condition.spindle, pl_data->spindle.rpm);
         }
+
 #ifdef KINEMATICS_API
+        if(pl_data->condition.jog_motion) {
+            sys_state_t state = state_get();
+            if ((state == STATE_IDLE || state == STATE_TOOL_CHANGE) && plan_get_current_block() != NULL) { // Check if there is a block to execute.
+                state_set(STATE_JOG);
+                st_prep_buffer();
+                st_wake_up();  // NOTE: Manual start. No state machine required.
+            }
+        }
+
+        pl_data->feed_rate = feed_rate;
       }
 #endif
     }
@@ -217,7 +232,6 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o
     float r_axis1 = -offset[plane.axis_1];
     float rt_axis0 = target[plane.axis_0] - center_axis0;
     float rt_axis1 = target[plane.axis_1] - center_axis1;
-
     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
     float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
 
@@ -292,7 +306,7 @@ void mc_arc (float *target, plan_line_data_t *pl_data, float *position, float *o
         float sin_Ti;
         float cos_Ti;
         float r_axisi;
-        uint32_t i, count = 0;
+        uint_fast16_t i, count = 0;
 
         for (i = 1; i < segments; i++) { // Increment (segments-1).
 
@@ -741,12 +755,15 @@ status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_bloc
 
     // Valid jog command. Plan, set state, and execute.
     mc_line(gc_block->values.xyz, pl_data);
+
+#ifndef KINEMATICS_API // kinematics may segment long jog moves triggering auto start (RUN)...
     sys_state_t state = state_get();
     if ((state == STATE_IDLE || state == STATE_TOOL_CHANGE) && plan_get_current_block() != NULL) { // Check if there is a block to execute.
         state_set(STATE_JOG);
         st_prep_buffer();
         st_wake_up();  // NOTE: Manual start. No state machine required.
     }
+#endif
 
     return Status_OK;
 }
@@ -796,6 +813,28 @@ status_code_t mc_homing_cycle (axes_signals_t cycle)
             return Status_Unhandled;
         }
 
+#ifdef KINEMATICS_API
+        if(kinematics.homing_cycle_validate) {
+
+            uint_fast8_t idx = N_AXIS;
+
+            if (!home_all) { // Perform homing cycle based on mask.
+                if(!kinematics.homing_cycle_validate(cycle)) {
+                    system_set_exec_alarm(Alarm_HomingFail);
+                    return Status_Unhandled;
+                }
+            } else do {
+                if(settings.homing.cycle[--idx].mask) {
+                    if(!kinematics.homing_cycle_validate(settings.homing.cycle[idx])) {
+                        system_set_exec_alarm(Alarm_HomingFail);
+                        return Status_Unhandled;
+                    }
+                }
+            } while(idx);
+
+        }
+#endif
+
         state_set(STATE_HOMING);                                // Set homing system state.
 #if COMPATIBILITY_LEVEL == 0
         protocol_enqueue_realtime_command(CMD_STATUS_REPORT);   // Force a status report and

planner.c

@@ -29,9 +29,7 @@
 #include "hal.h"
 #include "nuts_bolts.h"
 #include "planner.h"
-#ifdef KINEMATICS_API
-#include "kinematics.h"
-#endif
+
 #ifndef MINIMUM_JUNCTION_SPEED
 #define MINIMUM_JUNCTION_SPEED 0.0f
 #endif
@@ -394,10 +392,6 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
 
     // Compute and store initial move distance data.
 
-#ifdef KINEMATICS_API
-    kinematics.plan_target_to_steps(target_steps, target);
-#endif
-
     idx = N_AXIS;
     do {
         idx--;
@@ -405,9 +399,7 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
         // Also, compute individual axes distance for move and prep unit vector calculations.
         // NOTE: Computes true distance from converted step values.
 
-#ifndef KINEMATICS_API
         target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
-#endif
         delta_steps = target_steps[idx] - position_steps[idx];
         block->steps[idx] = labs(delta_steps);
         block->step_event_count = max(block->step_event_count, block->steps[idx]);
@@ -540,6 +532,22 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
 }
 
 
+// Get the planner position vectors.
+float *plan_get_position (void)
+{
+    static float position[N_AXIS];
+
+    uint_fast8_t idx = N_AXIS;
+
+    do {
+        idx--;
+        position[idx] = pl.position[idx] / settings.axis[idx].steps_per_mm;
+    } while(idx);
+
+    return position;
+}
+
+
 // Reset the planner position vectors. Called by the system abort/initialization routine.
 void plan_sync_position ()
 {

planner.h

@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2019-2021 Terje Io
+  Copyright (c) 2019-2022 Terje Io
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
   Copyright (c) 2009-2011 Simen Svale Skogsrud
 
@@ -132,6 +132,9 @@ float plan_get_exec_block_exit_speed_sqr();
 // Called by main program during planner calculations and step segment buffer during initialization.
 float plan_compute_profile_nominal_speed(plan_block_t *block);
 
+// Get the planner position vectors.
+float *plan_get_position (void);
+
 // Reset the planner position vector (in steps)
 void plan_sync_position();
 

protocol.c

@@ -805,6 +805,8 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c)
             if (state_get() & STATE_JOG) // Block all other states from invoking motion cancel.
                 system_set_exec_state_flag(EXEC_MOTION_CANCEL);
 #endif
+            if(grbl.on_jog_cancel)
+                grbl.on_jog_cancel(state_get());
             break;
 
         case CMD_GCODE_REPORT:

settings.c

@@ -77,6 +77,7 @@ PROGMEM const settings_t defaults = {
     .flags.legacy_rt_commands = DEFAULT_LEGACY_RTCOMMANDS,
     .flags.report_inches = DEFAULT_REPORT_INCHES,
     .flags.sleep_enable = DEFAULT_SLEEP_ENABLE,
+    .flags.compatibility_level = COMPATIBILITY_LEVEL,
 #if DISABLE_G92_PERSISTENCE
     .flags.g92_is_volatile = 1,
 #else
@@ -104,7 +105,13 @@ PROGMEM const settings_t defaults = {
     .steppers.step_invert.mask = DEFAULT_STEPPING_INVERT_MASK,
     .steppers.dir_invert.mask = DEFAULT_DIRECTION_INVERT_MASK,
     .steppers.ganged_dir_invert.mask = DEFAULT_GANGED_DIRECTION_INVERT_MASK,
+#if COMPATIBILITY_LEVEL <= 2
     .steppers.enable_invert.mask = INVERT_ST_ENABLE_MASK,
+#elif INVERT_ST_ENABLE_MASK
+    .steppers.enable_invert.mask = 0,
+#else
+    .steppers.enable_invert.mask = AXES_BITMASK,
+#endif
     .steppers.deenergize.mask = ST_DEENERGIZE_MASK,
 #if N_AXIS > 3
     .steppers.is_rotational.mask = (ST_ROTATIONAL_MASK & AXES_BITMASK) >> 3,
@@ -361,6 +368,9 @@ static char *get_linear_piece (setting_id_t id);
 #if N_AXIS > 3
 static status_code_t set_rotational_axes (setting_id_t id, uint_fast16_t int_value);
 #endif
+#if COMPATIBILITY_LEVEL > 2
+static status_code_t set_enable_invert_mask (setting_id_t id, uint_fast16_t int_value);
+#endif
 #if COMPATIBILITY_LEVEL > 1
 static status_code_t set_limits_invert_mask (setting_id_t id, uint_fast16_t int_value);
 #endif
@@ -384,7 +394,11 @@ PROGMEM static const setting_detail_t setting_detail[] = {
      { Setting_StepperIdleLockTime, Group_Stepper, "Step idle delay", "milliseconds", Format_Int16, "####0", NULL, "65535", Setting_IsLegacy, &settings.steppers.idle_lock_time, NULL, NULL },
      { Setting_StepInvertMask, Group_Stepper, "Step pulse invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.step_invert.mask, NULL, NULL },
      { Setting_DirInvertMask, Group_Stepper, "Step direction invert", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.dir_invert.mask, NULL, NULL },
+#if COMPATIBILITY_LEVEL <= 2
      { Setting_InvertStepperEnable, Group_Stepper, "Invert stepper enable pin(s)", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.steppers.enable_invert.mask, NULL, NULL },
+#else
+     { Setting_InvertStepperEnable, Group_Stepper, "Invert stepper enable pins", NULL, Format_Bool, NULL, NULL, NULL, Setting_IsLegacyFn, set_enable_invert_mask, get_int, NULL },
+#endif
 #if COMPATIBILITY_LEVEL <= 1
      { Setting_LimitPinsInvertMask, Group_Limits, "Invert limit pins", NULL, Format_AxisMask, NULL, NULL, NULL, Setting_IsLegacy, &settings.limits.invert.mask, NULL, NULL },
 #else
@@ -525,9 +539,13 @@ PROGMEM static const setting_descr_t setting_descr[] = {
     { Setting_StepperIdleLockTime, "Sets a short hold delay when stopping to let dynamics settle before disabling steppers. Value 255 keeps motors enabled." },
     { Setting_StepInvertMask, "Inverts the step signals (active low)." },
     { Setting_DirInvertMask, "Inverts the direction signals (active low)." },
+#if COMPATIBILITY_LEVEL <= 2
     { Setting_InvertStepperEnable, "Inverts the stepper driver enable signals. Most drivers uses active low enable requiring inversion.\\n\\n"
                                    "NOTE: If the stepper drivers shares the same enable signal only X is used."
     },
+#else
+    { Setting_InvertStepperEnable, "Inverts the stepper driver enable signals. Drivers using active high enable require inversion.\\n\\n" },
+#endif
     { Setting_LimitPinsInvertMask, "Inverts the axis limit input signals." },
     { Setting_InvertProbePin, "Inverts the probe input pin signal." },
     { Setting_SpindlePWMBehaviour, "" },
@@ -737,13 +755,26 @@ setting_details_t *settings_get_details (void)
     return &setting_details;
 }
 
+#if COMPATIBILITY_LEVEL > 2
+
+static status_code_t set_enable_invert_mask (setting_id_t id, uint_fast16_t int_value)
+{
+    settings.steppers.enable_invert.mask = int_value ? 0 : AXES_BITMASK;
+
+    return Status_OK;
+}
+
+#endif
+
 #if COMPATIBILITY_LEVEL > 1
+
 static status_code_t set_limits_invert_mask (setting_id_t id, uint_fast16_t int_value)
 {
     settings.limits.invert.mask = (int_value ? ~(INVERT_LIMIT_BIT_MASK) : INVERT_LIMIT_BIT_MASK) & AXES_BITMASK;
 
     return Status_OK;
 }
+
 #endif
 
 static status_code_t set_probe_invert (setting_id_t id, uint_fast16_t int_value)
@@ -1209,6 +1240,12 @@ static uint32_t get_int (setting_id_t id)
 
     switch(id) {
 
+#if COMPATIBILITY_LEVEL > 2
+        case Setting_InvertStepperEnable:
+            value = settings.steppers.enable_invert.mask ? 0 : 1;
+            break;
+#endif
+
 #if COMPATIBILITY_LEVEL > 1
         case Setting_LimitPinsInvertMask:
             value = settings.limits.invert.mask == INVERT_LIMIT_BIT_MASK ? 0 : 1;
@@ -1620,6 +1657,11 @@ bool read_global_settings ()
     settings.flags.g92_is_volatile = On;
 #endif
 
+#if COMPATIBILITY_LEVEL > 2
+    if(settings.steppers.enable_invert.mask)
+        settings.steppers.enable_invert.mask = AXES_BITMASK;
+#endif
+
     return ok && settings.version == SETTINGS_VERSION;
 }
 
@@ -1630,6 +1672,8 @@ void settings_write_global (void)
     if(override_backup.valid)
         restore_override_backup();
 
+    settings.flags.compatibility_level = COMPATIBILITY_LEVEL;
+
     if(hal.nvs.type != NVS_None)
         hal.nvs.memcpy_to_nvs(NVS_ADDR_GLOBAL, (uint8_t *)&settings, sizeof(settings_t), true);
 }

settings.h

@@ -334,7 +334,8 @@ typedef union {
                  restore_after_feed_hold         :1,
                  unused1                         :1,
                  g92_is_volatile                 :1,
-                 unassigned                      :6;
+                 compatibility_level             :4,
+                 unassigned                      :2;
     };
 } settingflags_t;
 

system.c

@@ -267,6 +267,10 @@ void system_command_help (void)
 #else
     hal.stream.write("$RST=# - reset offsets" ASCII_EOL);
 #endif
+    if(hal.spindle.reset_data)
+        hal.stream.write("$SR - reset spindle encoder data" ASCII_EOL);
+    if(hal.spindle.get_data)
+        hal.stream.write("$SD - output spindle encoder data" ASCII_EOL);
     hal.stream.write("$TLR - set tool offset reference" ASCII_EOL);
     hal.stream.write("$TPW - probe tool plate" ASCII_EOL);
     hal.stream.write("$EA - enumerate alarms" ASCII_EOL);
@@ -761,7 +765,7 @@ static status_code_t output_all_build_info (sys_state_t state, char *args)
     char info[sizeof(stored_line_t)];
 
     settings_read_build_info(info);
-    report_build_info(info, false);
+    report_build_info(info, true);
 
     return Status_OK;
 }
@@ -893,7 +897,7 @@ void system_flag_wco_change (void)
 void system_convert_array_steps_to_mpos (float *position, int32_t *steps)
 {
 #ifdef KINEMATICS_API
-    kinematics.convert_array_steps_to_mpos(position, steps);
+    kinematics.transform_steps_to_cartesian(position, steps);
 #else
     uint_fast8_t idx = N_AXIS;
     do {

tool_change.c

@@ -451,8 +451,10 @@ status_code_t tc_probe_workpiece (void)
     gc_parser_flags_t flags = {0};
     plan_line_data_t plan_data = {0};
 
+#if COMPATIBILITY_LEVEL <= 1
     if(probe_fixture)
         grbl.on_probe_fixture(next_tool, system_xy_at_fixture(CoordinateSystem_G59_3, TOOLSETTER_RADIUS), true);
+#endif
 
     // Get current position.
     system_convert_array_steps_to_mpos(target.values, sys.position);

wall_plotter.c

@@ -28,9 +28,13 @@
 
 #ifdef WALL_PLOTTER
 
+#include <math.h>
+#include <string.h>
+
 #include "settings.h"
 #include "planner.h"
 #include "kinematics.h"
+#include "hal.h"
 
 #define A_MOTOR X_AXIS // Must be X_AXIS
 #define B_MOTOR Y_AXIS // Must be Y_AXIS
@@ -52,11 +56,12 @@ typedef struct {
     float b;
 } coord_t;
 
+static bool jog_cancel = false;
 static machine_t machine = {0};
 
 // Returns machine position in mm converted from system position steps.
 // TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers.
-static void wp_convert_array_steps_to_mpos (float *position, int32_t *steps)
+static float *wp_convert_array_steps_to_mpos (float *position, int32_t *steps)
 {
     coord_t len;
 
@@ -67,71 +72,109 @@ static void wp_convert_array_steps_to_mpos (float *position, int32_t *steps)
     len.a = machine.width_mm - position[X_AXIS];
     position[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a );
     position[Z_AXIS] = steps[Z_AXIS] / settings.axis[Z_AXIS].steps_per_mm;
+
+    return position;
+}
+
+// Returns machine position in mm converted from system position steps.
+// TODO: perhaps change to double precision here - float calculation results in errors of a couple of micrometers.
+static float *transform_to_cartesian (float *target, float *position)
+{
+    coord_t len;
+
+    len.a = position[A_MOTOR];
+    len.b = position[B_MOTOR];
+
+    target[X_AXIS] = (machine.width_pow + len.a * len.a - len.b * len.b) / (2.0f * machine.width_mm);
+    len.a = machine.width_mm - target[X_AXIS];
+    target[Y_AXIS] = sqrtf(len.b * len.b - len.a * len.a );
+    target[Z_AXIS] = position[Z_AXIS];
+
+    return target;
 }
 
 // Wall plotter calculation only. Returns x or y-axis "steps" based on wall plotter motor steps.
 // A length = sqrt( X^2 + Y^2 )
 // B length = sqrt( (MACHINE_WIDTH - X)^2 + Y^2 )
-inline static int32_t wp_convert_to_a_motor_steps (float *target)
+inline static float wp_convert_to_a_motor_steps (float *target)
 {
-    return (int32_t)lroundf(sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[A_MOTOR].steps_per_mm);
+    return sqrtf(target[A_MOTOR] * target[A_MOTOR] + target[B_MOTOR] * target[B_MOTOR]);
 }
 
-inline static int32_t wp_convert_to_b_motor_steps (float *target)
+inline static float wp_convert_to_b_motor_steps (float *target)
 {
     float xpos = machine.width_mm - target[A_MOTOR];
-    return (int32_t)lroundf(sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]) * settings.axis[B_MOTOR].steps_per_mm);
+
+    return sqrtf(xpos * xpos + target[B_MOTOR] * target[B_MOTOR]);
 }
 
-// Transform absolute position from cartesian coordinate system (mm) to wall plotter coordinate system (step)
-static void wp_plan_target_to_steps (int32_t *target_steps, float *target)
+// Transform absolute position from cartesian coordinate system to wall plotter coordinate system
+static float *transform_from_cartesian (float *target, float *position)
 {
     uint_fast8_t idx = N_AXIS - 1;
 
     do {
-        target_steps[idx] = lroundf(target[idx] * settings.axis[idx].steps_per_mm);
+        target[idx] = position[idx];
     } while(--idx > Y_AXIS);
 
-    target_steps[A_MOTOR] = wp_convert_to_a_motor_steps(target);
-    target_steps[B_MOTOR] = wp_convert_to_b_motor_steps(target);
+    target[A_MOTOR] = wp_convert_to_a_motor_steps(position);
+    target[B_MOTOR] = wp_convert_to_b_motor_steps(position);
+
+    return target;
+}
+
+
+static inline float get_distance (float *p0, float *p1)
+{
+    uint_fast8_t idx = Z_AXIS;
+    float distance = 0.0f;
+
+    do {
+        idx--;
+        distance += (p0[idx] - p1[idx]) * (p0[idx] - p1[idx]);
+    } while(idx);
+
+    return sqrtf(distance);
 }
 
 // Wall plotter is circular in motion, so long lines must be divided up
-static bool wp_segment_line (float *target, plan_line_data_t *pl_data, bool init)
+static float *wp_segment_line (float *target, float *position, plan_line_data_t *pl_data, bool init)
 {
     static uint_fast16_t iterations;
     static bool segmented;
-    static float delta[N_AXIS], segment_target[N_AXIS];
+    static coord_data_t delta, segment_target, final_target, cpos;
 //    static plan_line_data_t plan;
 
     uint_fast8_t idx = N_AXIS;
 
     if(init) {
 
-        float max_delta = 0.0f;
+        jog_cancel = false;
 
-        do {
-            idx--;
-            delta[idx] = target[idx] - gc_state.position[idx];
-            max_delta = max(max_delta, fabsf(delta[idx]));
-        } while(idx);
+        memcpy(final_target.values, target, sizeof(final_target));
 
-        if((segmented = !(pl_data->condition.rapid_motion || pl_data->condition.jog_motion) &&
-                           max_delta > MAX_SEG_LENGTH_MM && !(delta[X_AXIS] == 0.0f && delta[Y_AXIS] == 0.0f))) {
+        transform_to_cartesian(segment_target.values, position);
+
+        delta.x = target[X_AXIS] - segment_target.x;
+        delta.y = target[Y_AXIS] - segment_target.y;
+        delta.z = target[Z_AXIS] - segment_target.z;
+
+        float distance = sqrtf(delta.x * delta.x + delta.y * delta.y);
+
+        if((segmented = !pl_data->condition.rapid_motion && distance > MAX_SEG_LENGTH_MM && !(delta.x == 0.0f && delta.y == 0.0f))) {
 
             idx = N_AXIS;
-            iterations = (uint_fast16_t)ceilf(max_delta / MAX_SEG_LENGTH_MM);
-
-            memcpy(segment_target, gc_state.position, sizeof(segment_target));
-//            memcpy(&plan, pl_data, sizeof(plan_line_data_t));
+            iterations = (uint_fast16_t)ceilf(distance / MAX_SEG_LENGTH_MM);
 
             do {
-                delta[--idx] /= (float)iterations;
-                target[idx] = gc_state.position[idx];
+                --idx;
+                delta.values[idx] = delta.values[idx] / (float)iterations;
             } while(idx);
 
-        } else
+        } else {
             iterations = 1;
+            memcpy(&segment_target, &final_target, sizeof(coord_data_t));
+        }
 
         iterations++; // return at least one iteration
 
@@ -139,16 +182,19 @@ static bool wp_segment_line (float *target, plan_line_data_t *pl_data, bool init
 
         iterations--;
 
-        if(segmented && iterations) do {
-            idx--;
-            segment_target[idx] += delta[idx];
-            target[idx] = segment_target[idx];
-//            memcpy(pl_data, &plan, sizeof(plan_line_data_t));
-        } while(idx);
+        if(segmented && iterations > 1) {
+            do {
+                idx--;
+                segment_target.values[idx] += delta.values[idx];
+            } while(idx);
 
+        } else
+            memcpy(&segment_target, &final_target, sizeof(coord_data_t));
+
+        transform_from_cartesian(cpos.values, segment_target.values);
     }
 
-    return iterations != 0;
+    return iterations == 0 || jog_cancel ? NULL : cpos.values;
 }
 
 
@@ -233,6 +279,10 @@ static void wp_limits_set_machine_positions (axes_signals_t cycle)
     } while(idx);
 }
 
+static void cancel_jog (sys_state_t state)
+{
+    jog_cancel = true;
+}
 
 // Initialize API pointers for Wall Plotter kinematics
 void wall_plotter_init (void)
@@ -253,9 +303,11 @@ void wall_plotter_init (void)
     kinematics.limits_set_target_pos = wp_limits_set_target_pos;
     kinematics.limits_get_axis_mask = wp_limits_get_axis_mask;
     kinematics.limits_set_machine_positions = wp_limits_set_machine_positions;
-    kinematics.plan_target_to_steps = wp_plan_target_to_steps;
-    kinematics.convert_array_steps_to_mpos = wp_convert_array_steps_to_mpos;
+    kinematics.transform_from_cartesian = transform_from_cartesian;
+    kinematics.transform_steps_to_cartesian = wp_convert_array_steps_to_mpos;
     kinematics.segment_line = wp_segment_line;
+
+    grbl.on_jog_cancel = cancel_jog;
 }
 
 #endif