Merge remote-tracking branch 'remotes/origin/M221_fix' into MK3_fast_dbg

Firmware/Configuration.h

@@ -8,7 +8,7 @@
 
 // Firmware version
 #define FW_VERSION "3.1.1"
-#define FW_COMMIT_NR   197
+#define FW_COMMIT_NR   201
 // FW_VERSION_UNKNOWN means this is an unofficial build.
 // The firmware should only be checked into github with this symbol.
 #define FW_DEV_VERSION FW_VERSION_UNKNOWN
@@ -133,6 +133,45 @@
 #define EEPROM_POWER_COUNT_TOT       (EEPROM_FERROR_COUNT_TOT - 2)             // uint16
 
 
+////////////////////////////////////////
+// TMC2130 Accurate sensorless homing 
+
+// X-axis home origin (stepper phase in microsteps, 0..63 for 16ustep resolution)
+#define EEPROM_TMC2130_HOME_X_ORIGIN           (EEPROM_POWER_COUNT_TOT - 1)                    // uint8
+// X-axis home bsteps (number of microsteps backward)
+#define EEPROM_TMC2130_HOME_X_BSTEPS           (EEPROM_TMC2130_HOME_X_ORIGIN - 1)              // uint8
+// X-axis home fsteps (number of microsteps forward)
+#define EEPROM_TMC2130_HOME_X_FSTEPS           (EEPROM_TMC2130_HOME_X_BSTEPS - 1)              // uint8
+// Y-axis home origin (stepper phase in microsteps, 0..63 for 16ustep resolution)
+#define EEPROM_TMC2130_HOME_Y_ORIGIN           (EEPROM_TMC2130_HOME_X_FSTEPS - 1)              // uint8
+// X-axis home bsteps (number of microsteps backward)
+#define EEPROM_TMC2130_HOME_Y_BSTEPS           (EEPROM_TMC2130_HOME_Y_ORIGIN - 1)              // uint8
+// X-axis home fsteps (number of microsteps forward)
+#define EEPROM_TMC2130_HOME_Y_FSTEPS           (EEPROM_TMC2130_HOME_Y_BSTEPS - 1)              // uint8
+// Accurate homing enabled
+#define EEPROM_TMC2130_HOME_ENABLED            (EEPROM_TMC2130_HOME_Y_FSTEPS - 1)              // uint8
+
+
+////////////////////////////////////////
+// TMC2130 uStep linearity correction
+
+// Linearity correction factor (XYZE) encoded as uint8 (0=>1, 1=>1.001, 254=>1.254, 255=>clear eeprom/disabled)
+#define EEPROM_TMC2130_WAVE_X_FAC              (EEPROM_TMC2130_HOME_ENABLED - 1)               // uint8
+#define EEPROM_TMC2130_WAVE_Y_FAC              (EEPROM_TMC2130_WAVE_X_FAC - 1)                 // uint8
+#define EEPROM_TMC2130_WAVE_Z_FAC              (EEPROM_TMC2130_WAVE_Y_FAC - 1)                 // uint8
+#define EEPROM_TMC2130_WAVE_E_FAC              (EEPROM_TMC2130_WAVE_Z_FAC - 1)                 // uint8
+
+
+////////////////////////////////////////
+// TMC2130 uStep resolution
+
+// microstep resolution (XYZE): usteps = (256 >> mres)
+#define EEPROM_TMC2130_X_MRES              (EEPROM_TMC2130_WAVE_E_FAC - 1)                     // uint8
+#define EEPROM_TMC2130_Y_MRES              (EEPROM_TMC2130_X_MRES - 1)                         // uint8
+#define EEPROM_TMC2130_Z_MRES              (EEPROM_TMC2130_Y_MRES - 1)                         // uint8
+#define EEPROM_TMC2130_E_MRES              (EEPROM_TMC2130_Z_MRES - 1)                         // uint8
+
+
 //TMC2130 configuration
 #define EEPROM_TMC_AXIS_SIZE  //axis configuration block size
 #define EEPROM_TMC_X (EEPROM_TMC + 0 * EEPROM_TMC_AXIS_SIZE) //X axis configuration blok
@@ -792,34 +831,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //
 //#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command
 
-/**********************************************************************\
- * Support for a filament diameter sensor
- * Also allows adjustment of diameter at print time (vs  at slicing)
- * Single extruder only at this point (extruder 0)
- * 
- * Motherboards
- * 34 - RAMPS1.4 - uses Analog input 5 on the AUX2 connector 
- * 81 - Printrboard - Uses Analog input 2 on the Exp1 connector (version B,C,D,E)
- * 301 - Rambo  - uses Analog input 3
- * Note may require analog pins to be defined for different motherboards
- **********************************************************************/
-// Uncomment below to enable
-//#define FILAMENT_SENSOR
-
-#define FILAMENT_SENSOR_EXTRUDER_NUM	0  //The number of the extruder that has the filament sensor (0,1,2)
-#define MEASUREMENT_DELAY_CM			14  //measurement delay in cm.  This is the distance from filament sensor to middle of barrel
-
-#define DEFAULT_NOMINAL_FILAMENT_DIA  3.0  //Enter the diameter (in mm) of the filament generally used (3.0 mm or 1.75 mm) - this is then used in the slicer software.  Used for sensor reading validation
-#define MEASURED_UPPER_LIMIT          3.30  //upper limit factor used for sensor reading validation in mm
-#define MEASURED_LOWER_LIMIT          1.90  //lower limit factor for sensor reading validation in mm
-#define MAX_MEASUREMENT_DELAY			20  //delay buffer size in bytes (1 byte = 1cm)- limits maximum measurement delay allowable (must be larger than MEASUREMENT_DELAY_CM  and lower number saves RAM)
-
-//defines used in the code
-#define DEFAULT_MEASURED_FILAMENT_DIA  DEFAULT_NOMINAL_FILAMENT_DIA  //set measured to nominal initially 
-
-//When using an LCD, uncomment the line below to display the Filament sensor data on the last line instead of status.  Status will appear for 5 sec.
-//#define FILAMENT_LCD_DISPLAY
-
+#define DEFAULT_NOMINAL_FILAMENT_DIA  1.75  //Enter the diameter (in mm) of the filament generally used (3.0 mm or 1.75 mm). Used by the volumetric extrusion.
 
 // Calibration status of the machine, to be stored into the EEPROM,
 // (unsigned char*)EEPROM_CALIBRATION_STATUS

Firmware/ConfigurationStore.cpp

@@ -136,10 +136,12 @@ void Config_StoreSettings(uint16_t offset, uint8_t level)
   }
 #endif //LIN_ADVANCE
 
-  /*MYSERIAL.print("Top address used:\n");
+/*  MYSERIAL.print("Top address used:\n");
   MYSERIAL.print(i); 
-  MYSERIAL.print("\n");
-  */
+  MYSERIAL.print("; (0x");
+  MYSERIAL.print(i, HEX);
+  MYSERIAL.println(")");
+*/ 
   char ver2[4]=EEPROM_VERSION;
   i=offset;
   EEPROM_WRITE_VAR(i,ver2); // validate data
@@ -470,7 +472,7 @@ void Config_ResetDefault()
 	filament_size[2] = DEFAULT_NOMINAL_FILAMENT_DIA;
 #endif
 #endif
-	calculate_volumetric_multipliers();
+	calculate_extruder_multipliers();
 
 SERIAL_ECHO_START;
 SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");

Firmware/Configuration_prusa.h

@@ -203,8 +203,8 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define TMC2130_SG_THRS_E       3     // stallguard sensitivity for E axis
 
 //new settings is possible for vsense = 1, running current value > 31 set vsense to zero and shift both currents by 1 bit right (Z axis only)
-#define TMC2130_CURRENTS_H {13, 20, 25, 35}  // default holding currents for all axes
-#define TMC2130_CURRENTS_R {13, 20, 25, 35}  // default running currents for all axes
+#define TMC2130_CURRENTS_H {16, 20, 28, 36}  // default holding currents for all axes
+#define TMC2130_CURRENTS_R {16, 20, 28, 36}  // default running currents for all axes
 #define TMC2130_UNLOAD_CURRENT_R 12			 // lowe current for M600 to protect filament sensor 
 
 //#define TMC2130_DEBUG

Firmware/Dcodes.cpp

@@ -447,11 +447,8 @@ void dcode_10()
 }
 
 void dcode_12()
-{//Reset Filament error, Power loss and crash counter ( Do it before every print and you can get stats for the print )
-	LOG("D12 - Reset failstat counters\n");
-    eeprom_update_byte((uint8_t*)EEPROM_CRASH_COUNT_X, 0x00);
-    eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, 0x00);
-    eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, 0x00);
+{//Time
+	LOG("D12 - Time\n");
 }
 
 #include "tmc2130.h"
@@ -461,28 +458,101 @@ extern void st_synchronize();
 
 void dcode_2130()
 {
-//	printf("test");
 	printf_P(PSTR("D2130 - TMC2130\n"));
 	uint8_t axis = 0xff;
-	if (code_seen('X'))
-		axis = X_AXIS;
-	else if (code_seen('Y'))
-		axis = Y_AXIS;
+	switch (strchr_pointer[1+4])
+	{
+	case 'X': axis = X_AXIS; break;
+	case 'Y': axis = Y_AXIS; break;
+	case 'Z': axis = Z_AXIS; break;
+	case 'E': axis = E_AXIS; break;
+	}
 	if (axis != 0xff)
 	{
-		homeaxis(axis);
-		tmc2130_sg_meassure_start(axis);
-		memcpy(destination, current_position, sizeof(destination));
-        destination[axis] = 200;
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
-        st_synchronize();
-		memcpy(destination, current_position, sizeof(destination));
-        destination[axis] = 0;
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
-        st_synchronize();
-		uint16_t sg = tmc2130_sg_meassure_stop();
-		tmc2130_sg_meassure = 0xff;
-		printf_P(PSTR("Meassure avg = %d\n"), sg);
+		char ch_axis = strchr_pointer[1+4];
+		if (strchr_pointer[1+5] == '0') { tmc2130_set_pwr(axis, 0); }
+		else if (strchr_pointer[1+5] == '1') { tmc2130_set_pwr(axis, 1); }
+		else if (strchr_pointer[1+5] == '+')
+		{
+			if (strchr_pointer[1+6] == 0)
+			{
+				tmc2130_set_dir(axis, 0);
+				tmc2130_do_step(axis);
+			}
+			else
+			{
+				uint8_t steps = atoi(strchr_pointer + 1 + 6);
+				tmc2130_do_steps(axis, steps, 0, 1000);
+			}
+		}
+		else if (strchr_pointer[1+5] == '-')
+		{
+			if (strchr_pointer[1+6] == 0)
+			{
+				tmc2130_set_dir(axis, 1);
+				tmc2130_do_step(axis);
+			}
+			else
+			{
+				uint8_t steps = atoi(strchr_pointer + 1 + 6);
+				tmc2130_do_steps(axis, steps, 1, 1000);
+			}
+		}
+		else if (strchr_pointer[1+5] == '?')
+		{
+			if (strcmp(strchr_pointer + 7, "mres") == 0) printf_P(PSTR("%c mres=%d\n"), ch_axis, tmc2130_mres[axis]);
+			else if (strcmp(strchr_pointer + 7, "step") == 0) printf_P(PSTR("%c step=%d\n"), ch_axis, tmc2130_rd_MSCNT(axis) >> tmc2130_mres[axis]);
+			else if (strcmp(strchr_pointer + 7, "mscnt") == 0) printf_P(PSTR("%c MSCNT=%d\n"), ch_axis, tmc2130_rd_MSCNT(axis));
+			else if (strcmp(strchr_pointer + 7, "mscuract") == 0)
+			{
+				uint32_t val = tmc2130_rd_MSCURACT(axis);
+				int curA = (val & 0xff);
+				int curB = ((val >> 16) & 0xff);
+				if ((val << 7) & 0x8000) curA -= 256;
+				if ((val >> 9) & 0x8000) curB -= 256;
+				printf_P(PSTR("%c MSCURACT=0x%08lx A=%d B=%d\n"), ch_axis, val, curA, curB);
+			}
+			else if (strcmp(strchr_pointer + 7, "wave") == 0)
+			{
+				tmc2130_get_wave(axis, 0, stdout);
+			}
+		}
+		else if (strchr_pointer[1+5] == '!')
+		{
+			if (strncmp(strchr_pointer + 7, "step", 4) == 0)
+			{
+				uint8_t step = atoi(strchr_pointer + 11);
+				uint16_t res = tmc2130_get_res(axis);
+				tmc2130_goto_step(axis, step & (4*res - 1), 2, 1000, res);
+			}
+			else if (strncmp(strchr_pointer + 7, "mres", 4) == 0)
+			{
+				uint8_t mres = strchr_pointer[11] - '0';
+				if ((mres >= 0) && (mres <= 8))
+				{
+					st_synchronize();
+					uint16_t res = tmc2130_get_res(axis);
+					uint16_t res_new = tmc2130_mres2usteps(mres);
+					tmc2130_set_res(axis, res_new);
+					if (res_new > res)
+						axis_steps_per_unit[axis] *= (res_new / res);
+					else
+						axis_steps_per_unit[axis] /= (res / res_new);
+				}
+			}
+			else if (strncmp(strchr_pointer + 7, "wave", 4) == 0)
+			{
+				uint8_t fac200 = atoi(strchr_pointer + 11) & 0xff;
+				if (fac200 < TMC2130_WAVE_FAC200_MIN) fac200 = 0;
+				if (fac200 > TMC2130_WAVE_FAC200_MAX) fac200 = TMC2130_WAVE_FAC200_MAX;
+				tmc2130_set_wave(axis, 247, fac200);
+				tmc2130_wave_fac[axis] = fac200;
+			}
+		}
+		else if (strchr_pointer[1+5] == '@')
+		{
+			tmc2130_home_calibrate(axis);
+		}
 	}
 }
 

Firmware/Dcodes.h

@@ -15,7 +15,6 @@ extern void dcode_8(); //D8 - Read/Write PINDA
 extern void dcode_9(); //D9 - Read/Write ADC (Write=enable simulated, Read=disable simulated)
 
 extern void dcode_10(); //D10 - XYZ calibration = OK
-extern void dcode_12(); //D12 - Reset failstat counters
 
 extern void dcode_2130(); //D2130 - TMC2130
 extern void dcode_9125(); //D9125 - PAT9125

Firmware/Marlin.h

@@ -276,24 +276,13 @@ extern float max_pos[3];
 extern bool axis_known_position[3];
 extern float zprobe_zoffset;
 extern int fanSpeed;
-extern void homeaxis(int axis);
+extern void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
 
 
 #ifdef FAN_SOFT_PWM
 extern unsigned char fanSpeedSoftPwm;
 #endif
 
-
-#ifdef FILAMENT_SENSOR
-  extern float filament_width_nominal;  //holds the theoretical filament diameter ie., 3.00 or 1.75
-  extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion
-  extern float filament_width_meas; //holds the filament diameter as accurately measured
-  extern signed char measurement_delay[];  //ring buffer to delay measurement
-  extern int delay_index1, delay_index2;  //index into ring buffer
-  extern float delay_dist; //delay distance counter
-  extern int meas_delay_cm; //delay distance
-#endif
-
 #ifdef FWRETRACT
 extern bool autoretract_enabled;
 extern bool retracted[EXTRUDERS];
@@ -358,7 +347,7 @@ extern bool sortAlpha;
 
 extern char dir_names[3][9];
 
-extern void calculate_volumetric_multipliers();
+extern void calculate_extruder_multipliers();
 
 // Similar to the default Arduino delay function, 
 // but it keeps the background tasks running.
@@ -379,6 +368,7 @@ float temp_comp_interpolation(float temperature);
 void temp_compensation_apply();
 void temp_compensation_start();
 void show_fw_version_warnings();
+void erase_eeprom_section(uint16_t offset, uint16_t bytes);
 
 #ifdef PINDA_THERMISTOR
 float temp_compensation_pinda_thermistor_offset(float temperature_pinda);

Firmware/Marlin_main.cpp

@@ -333,7 +333,7 @@ float filament_size[EXTRUDERS] = { DEFAULT_NOMINAL_FILAMENT_DIA
     #endif
   #endif
 };
-float volumetric_multiplier[EXTRUDERS] = {1.0
+float extruder_multiplier[EXTRUDERS] = {1.0
   #if EXTRUDERS > 1
     , 1.0
     #if EXTRUDERS > 2
@@ -410,18 +410,6 @@ bool cancel_heatup = false ;
   #define KEEPALIVE_STATE(n);
 #endif
 
-#ifdef FILAMENT_SENSOR
-  //Variables for Filament Sensor input 
-  float filament_width_nominal=DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404 
-  bool filament_sensor=false;  //M405 turns on filament_sensor control, M406 turns it off 
-  float filament_width_meas=DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter 
-  signed char measurement_delay[MAX_MEASUREMENT_DELAY+1];  //ring buffer to delay measurement  store extruder factor after subtracting 100 
-  int delay_index1=0;  //index into ring buffer
-  int delay_index2=-1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
-  float delay_dist=0; //delay distance counter  
-  int meas_delay_cm = MEASUREMENT_DELAY_CM;  //distance delay setting
-#endif
-
 const char errormagic[] PROGMEM = "Error:";
 const char echomagic[] PROGMEM = "echo:";
 
@@ -930,6 +918,13 @@ void show_fw_version_warnings() {
 	lcd_update_enable(true);
 }
 
+
+
+void erase_eeprom_section(uint16_t offset, uint16_t bytes)
+{
+	for (int i = offset; i < (offset+bytes); i++) eeprom_write_byte((uint8_t*)i, 0xFF);
+}
+
 // "Setup" function is called by the Arduino framework on startup.
 // Before startup, the Timers-functions (PWM)/Analog RW and HardwareSerial provided by the Arduino-code 
 // are initialized by the main() routine provided by the Arduino framework.
@@ -1024,6 +1019,7 @@ void setup()
 
 #ifdef TMC2130
 	uint8_t silentMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
+	if (silentMode == 0xff) silentMode = 0;
 	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
 	uint8_t crashdet = eeprom_read_byte((uint8_t*)EEPROM_CRASH_DET);
 	if (crashdet)
@@ -1037,6 +1033,28 @@ void setup()
 	    MYSERIAL.println("CrashDetect DISABLED");
 	}
 
+	tmc2130_wave_fac[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_X_FAC);
+	tmc2130_wave_fac[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Y_FAC);
+	tmc2130_wave_fac[Z_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Z_FAC);
+	tmc2130_wave_fac[E_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_E_FAC);
+	if (tmc2130_wave_fac[X_AXIS] == 0xff) tmc2130_wave_fac[X_AXIS] = 0;
+	if (tmc2130_wave_fac[Y_AXIS] == 0xff) tmc2130_wave_fac[Y_AXIS] = 0;
+	if (tmc2130_wave_fac[Z_AXIS] == 0xff) tmc2130_wave_fac[Z_AXIS] = 0;
+	if (tmc2130_wave_fac[E_AXIS] == 0xff) tmc2130_wave_fac[E_AXIS] = 0;
+
+	tmc2130_mres[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_X_MRES);
+	tmc2130_mres[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Y_MRES);
+	tmc2130_mres[Z_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Z_MRES);
+	tmc2130_mres[E_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_E_MRES);
+	if (tmc2130_mres[X_AXIS] == 0xff) tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	if (tmc2130_mres[Y_AXIS] == 0xff) tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	if (tmc2130_mres[Z_AXIS] == 0xff) tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
+	if (tmc2130_mres[E_AXIS] == 0xff) tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_X_MRES, tmc2130_mres[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Y_MRES, tmc2130_mres[Y_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Z_MRES, tmc2130_mres[Z_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_E_MRES, tmc2130_mres[E_AXIS]);
+
 #endif //TMC2130
 
 	st_init();    // Initialize stepper, this enables interrupts!
@@ -1064,19 +1082,19 @@ void setup()
 	setup_homepin();
 
   if (1) {
-///    SERIAL_ECHOPGM("initial zsteps on power up: "); MYSERIAL.println(tmc2130_rd_MSCNT(Z_TMC2130_CS));
+///    SERIAL_ECHOPGM("initial zsteps on power up: "); MYSERIAL.println(tmc2130_rd_MSCNT(Z_AXIS));
     // try to run to zero phase before powering the Z motor.    
     // Move in negative direction
     WRITE(Z_DIR_PIN,INVERT_Z_DIR);
     // Round the current micro-micro steps to micro steps.
-    for (uint16_t phase = (tmc2130_rd_MSCNT(Z_TMC2130_CS) + 8) >> 4; phase > 0; -- phase) {
+    for (uint16_t phase = (tmc2130_rd_MSCNT(Z_AXIS) + 8) >> 4; phase > 0; -- phase) {
       // Until the phase counter is reset to zero.
       WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
       delay(2);
       WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
       delay(2);
     }
-//    SERIAL_ECHOPGM("initial zsteps after reset: "); MYSERIAL.println(tmc2130_rd_MSCNT(Z_TMC2130_CS));
+//    SERIAL_ECHOPGM("initial zsteps after reset: "); MYSERIAL.println(tmc2130_rd_MSCNT(Z_AXIS));
   }
 
 #if defined(Z_AXIS_ALWAYS_ON)
@@ -1172,8 +1190,10 @@ void setup()
 
   show_fw_version_warnings();
 
-  if (!previous_settings_retrieved) lcd_show_fullscreen_message_and_wait_P(MSG_DEFAULT_SETTINGS_LOADED); //if EEPROM version was changed, inform user that default setting were loaded
-
+  if (!previous_settings_retrieved) {
+	  lcd_show_fullscreen_message_and_wait_P(MSG_DEFAULT_SETTINGS_LOADED); //if EEPROM version was changed, inform user that default setting were loaded
+	  erase_eeprom_section(EEPROM_OFFSET, 156); 							   //erase M500 part of eeprom
+  }
   if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) == 1) {
 	  lcd_wizard(0);
   }
@@ -1207,7 +1227,24 @@ void setup()
   // Store the currently running firmware into an eeprom,
   // so the next time the firmware gets updated, it will know from which version it has been updated.
   update_current_firmware_version_to_eeprom();
-  
+
+  	tmc2130_home_origin[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN);
+	tmc2130_home_bsteps[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_X_BSTEPS);
+	tmc2130_home_fsteps[X_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_X_FSTEPS);
+	if (tmc2130_home_origin[X_AXIS] == 0xff) tmc2130_home_origin[X_AXIS] = 0;
+	if (tmc2130_home_bsteps[X_AXIS] == 0xff) tmc2130_home_bsteps[X_AXIS] = 48;
+	if (tmc2130_home_fsteps[X_AXIS] == 0xff) tmc2130_home_fsteps[X_AXIS] = 48;
+
+	tmc2130_home_origin[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN);
+	tmc2130_home_bsteps[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_BSTEPS);
+	tmc2130_home_fsteps[Y_AXIS] = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_FSTEPS);
+	if (tmc2130_home_origin[Y_AXIS] == 0xff) tmc2130_home_origin[Y_AXIS] = 0;
+	if (tmc2130_home_bsteps[Y_AXIS] == 0xff) tmc2130_home_bsteps[Y_AXIS] = 48;
+	if (tmc2130_home_fsteps[Y_AXIS] == 0xff) tmc2130_home_fsteps[Y_AXIS] = 48;
+
+	tmc2130_home_enabled = eeprom_read_byte((uint8_t*)EEPROM_TMC2130_HOME_ENABLED);
+	if (tmc2130_home_enabled == 0xff) tmc2130_home_enabled = 0;
+
   if (eeprom_read_byte((uint8_t*)EEPROM_UVLO) == 1) { //previous print was terminated by UVLO
 /*
 	  if (lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_RECOVER_PRINT, false))	recover_print();
@@ -1776,9 +1813,9 @@ bool calibrate_z_auto()
 }
 #endif //TMC2130
 
-void homeaxis(int axis)
+void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
 {
-	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homming
+	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homing
 #define HOMEAXIS_DO(LETTER) \
 ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
     if ((axis==X_AXIS)?HOMEAXIS_DO(X):(axis==Y_AXIS)?HOMEAXIS_DO(Y):0)
@@ -1788,7 +1825,8 @@ void homeaxis(int axis)
 
 #ifdef TMC2130
     	tmc2130_home_enter(X_AXIS_MASK << axis);
-#endif
+#endif //TMC2130
+
 
         // Move right a bit, so that the print head does not touch the left end position,
         // and the following left movement has a chance to achieve the required velocity
@@ -1812,44 +1850,66 @@ void homeaxis(int axis)
         destination[axis] = - 1.1 * max_length(axis);
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
-        // Move right from the collision to a known distance from the left end stop with the collision detection disabled.
-        endstops_hit_on_purpose();
-        enable_endstops(false);
-        current_position[axis] = 0;
-        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-        destination[axis] = 10.f;
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        st_synchronize();
-        endstops_hit_on_purpose();
-        // Now move left up to the collision, this time with a repeatable velocity.
-        enable_endstops(true);
-        destination[axis] = - 15.f;
-        feedrate = homing_feedrate[axis]/2;
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        st_synchronize();
+		for (uint8_t i = 0; i < cnt; i++)
+		{
+			// Move right from the collision to a known distance from the left end stop with the collision detection disabled.
+			endstops_hit_on_purpose();
+			enable_endstops(false);
+			current_position[axis] = 0;
+			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			destination[axis] = 10.f;
+			plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+			st_synchronize();
+			endstops_hit_on_purpose();
+			// Now move left up to the collision, this time with a repeatable velocity.
+			enable_endstops(true);
+			destination[axis] = - 11.f;
+#ifdef TMC2130
+			feedrate = homing_feedrate[axis];
+#else //TMC2130
+			feedrate = homing_feedrate[axis] / 2;
+#endif //TMC2130
+			plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+			st_synchronize();
+#ifdef TMC2130
+			uint16_t mscnt = tmc2130_rd_MSCNT(axis);
+			if (pstep) pstep[i] = mscnt >> 4;
+			printf_P(PSTR("%3d step=%2d mscnt=%4d\n"), i, mscnt >> 4, mscnt);
+#endif //TMC2130
+		}
+		endstops_hit_on_purpose();
+		enable_endstops(false);
+
+#ifdef TMC2130
+		uint8_t orig = tmc2130_home_origin[axis];
+		uint8_t back = tmc2130_home_bsteps[axis];
+		if (tmc2130_home_enabled && (orig <= 63))
+		{
+			tmc2130_goto_step(axis, orig, 2, 1000, tmc2130_get_res(axis));
+			if (back > 0)
+				tmc2130_do_steps(axis, back, 1, 1000);
+		}
+		else
+			tmc2130_do_steps(axis, 8, 2, 1000);
+		tmc2130_home_exit();
+#endif //TMC2130
 
         axis_is_at_home(axis);
         axis_known_position[axis] = true;
-
+        // Move from minimum
 #ifdef TMC2130
-        tmc2130_home_exit();
-#endif
-        // Move the X carriage away from the collision.
-        // If this is not done, the X cariage will jump from the collision at the instant the Trinamic driver reduces power on idle.
-        endstops_hit_on_purpose();
-        enable_endstops(false);
-        {
-          // Two full periods (4 full steps).
-          float gap = 0.32f * 2.f;
-          current_position[axis] -= gap;
-          plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-          current_position[axis] += gap;
-        }
+        float dist = 0.01f * tmc2130_home_fsteps[axis];
+#else //TMC2130
+        float dist = 0.01f * 64;
+#endif //TMC2130
+        current_position[axis] -= dist;
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        current_position[axis] += dist;
         destination[axis] = current_position[axis];
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], 0.3f*feedrate/60, active_extruder);
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], 0.5f*feedrate/60, active_extruder);
         st_synchronize();
 
-    		feedrate = 0.0;
+   		feedrate = 0.0;
     }
     else if ((axis==Z_AXIS)?HOMEAXIS_DO(Z):0)
 	{
@@ -1900,11 +1960,7 @@ void refresh_cmd_timeout(void)
       destination[Y_AXIS]=current_position[Y_AXIS];
       destination[Z_AXIS]=current_position[Z_AXIS];
       destination[E_AXIS]=current_position[E_AXIS];
-      if (swapretract) {
-        current_position[E_AXIS]+=retract_length_swap/volumetric_multiplier[active_extruder];
-      } else {
-        current_position[E_AXIS]+=retract_length/volumetric_multiplier[active_extruder];
-      }
+      current_position[E_AXIS]+=(swapretract?retract_length_swap:retract_length)*float(extrudemultiply)*0.01f;
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
       feedrate=retract_feedrate*60;
@@ -1921,12 +1977,7 @@ void refresh_cmd_timeout(void)
       destination[E_AXIS]=current_position[E_AXIS];
       current_position[Z_AXIS]+=retract_zlift;
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-      //prepare_move();
-      if (swapretract) {
-        current_position[E_AXIS]-=(retract_length_swap+retract_recover_length_swap)/volumetric_multiplier[active_extruder]; 
-      } else {
-        current_position[E_AXIS]-=(retract_length+retract_recover_length)/volumetric_multiplier[active_extruder]; 
-      }
+      current_position[E_AXIS]-=(swapretract?(retract_length_swap+retract_recover_length_swap):(retract_length+retract_recover_length))*float(extrudemultiply)*0.01f;
       plan_set_e_position(current_position[E_AXIS]);
       float oldFeedrate = feedrate;
       feedrate=retract_recover_feedrate*60;
@@ -2689,6 +2740,8 @@ void process_commands()
       bool home_x = code_seen(axis_codes[X_AXIS]);
       bool home_y = code_seen(axis_codes[Y_AXIS]);
       bool home_z = code_seen(axis_codes[Z_AXIS]);
+      // calibrate?
+      bool calib = code_seen('C');
       // Either all X,Y,Z codes are present, or none of them.
       bool home_all_axes = home_x == home_y && home_x == home_z;
       if (home_all_axes)
@@ -2773,10 +2826,20 @@ void process_commands()
 
 	 
       if(home_x)
-        homeaxis(X_AXIS);
+	  {
+		if (!calib)
+			homeaxis(X_AXIS);
+		else
+			tmc2130_home_calibrate(X_AXIS);
+	  }
 
       if(home_y)
-        homeaxis(Y_AXIS);
+	  {
+		if (!calib)
+	        homeaxis(Y_AXIS);
+		else
+			tmc2130_home_calibrate(Y_AXIS);
+	  }
 
       if(code_seen(axis_codes[X_AXIS]) && code_value_long() != 0)
         current_position[X_AXIS]=code_value()+add_homing[X_AXIS];
@@ -4977,7 +5040,7 @@ Sigma_Exit:
           //reserved for setting filament diameter via UFID or filament measuring device
           break;
         }
-		calculate_volumetric_multipliers();
+		calculate_extruder_multipliers();
       }
       break;
     case 201: // M201
@@ -5145,6 +5208,7 @@ Sigma_Exit:
           extrudemultiply = tmp_code ;
         }
       }
+      calculate_extruder_multipliers();
     }
     break;
 
@@ -5383,69 +5447,6 @@ Sigma_Exit:
     }
     break;
 
-#ifdef FILAMENT_SENSOR
-case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width 
-    {
-    #if (FILWIDTH_PIN > -1) 
-    if(code_seen('N')) filament_width_nominal=code_value();
-    else{
-    SERIAL_PROTOCOLPGM("Filament dia (nominal mm):"); 
-    SERIAL_PROTOCOLLN(filament_width_nominal); 
-    }
-    #endif
-    }
-    break; 
-    
-    case 405:  //M405 Turn on filament sensor for control 
-    {
-    
-    
-    if(code_seen('D')) meas_delay_cm=code_value();
-       
-       if(meas_delay_cm> MAX_MEASUREMENT_DELAY)
-       	meas_delay_cm = MAX_MEASUREMENT_DELAY;
-    
-       if(delay_index2 == -1)  //initialize the ring buffer if it has not been done since startup
-    	   {
-    	   int temp_ratio = widthFil_to_size_ratio(); 
-       	    
-       	    for (delay_index1=0; delay_index1<(MAX_MEASUREMENT_DELAY+1); ++delay_index1 ){
-       	              measurement_delay[delay_index1]=temp_ratio-100;  //subtract 100 to scale within a signed byte
-       	        }
-       	    delay_index1=0;
-       	    delay_index2=0;	
-    	   }
-    
-    filament_sensor = true ; 
-    
-    //SERIAL_PROTOCOLPGM("Filament dia (measured mm):"); 
-    //SERIAL_PROTOCOL(filament_width_meas); 
-    //SERIAL_PROTOCOLPGM("Extrusion ratio(%):"); 
-    //SERIAL_PROTOCOL(extrudemultiply); 
-    } 
-    break; 
-    
-    case 406:  //M406 Turn off filament sensor for control 
-    {      
-    filament_sensor = false ; 
-    } 
-    break; 
-  
-    case 407:   //M407 Display measured filament diameter 
-    { 
-     
-    
-    
-    SERIAL_PROTOCOLPGM("Filament dia (measured mm):"); 
-    SERIAL_PROTOCOLLN(filament_width_meas);   
-    } 
-    break; 
-    #endif
-    
-
-
-
-
     case 500: // M500 Store settings in EEPROM
     {
         Config_StoreSettings(EEPROM_OFFSET);
@@ -6111,12 +6112,38 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 
     case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
     {
+	#ifdef TMC2130
+		if(code_seen('E'))
+		{
+			uint16_t res_new = code_value();
+			if ((res_new == 8) || (res_new == 16) || (res_new == 32) || (res_new == 64) || (res_new == 128))
+			{
+				st_synchronize();
+				uint8_t axis = E_AXIS;
+				uint16_t res = tmc2130_get_res(axis);
+				tmc2130_set_res(axis, res_new);
+				if (res_new > res)
+				{
+					uint16_t fac = (res_new / res);
+					axis_steps_per_unit[axis] *= fac;
+					position[E_AXIS] *= fac;
+				}
+				else
+				{
+					uint16_t fac = (res / res_new);
+					axis_steps_per_unit[axis] /= fac;
+					position[E_AXIS] /= fac;
+				}
+			}
+		}
+	#else //TMC2130
       #if defined(X_MS1_PIN) && X_MS1_PIN > -1
         if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
         for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
         if(code_seen('B')) microstep_mode(4,code_value());
         microstep_readings();
       #endif
+	#endif //TMC2130
     }
     break;
     case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
@@ -6361,9 +6388,6 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 	case 10: // D10 - XYZ calibration = OK
 		dcode_10(); break;
     
-    case 12: //D12 - Reset failstat counters
-		dcode_12(); break;
-
 	case 2130: // D9125 - TMC2130
 		dcode_2130(); break;
 	case 9125: // D9125 - PAT9125
@@ -6408,7 +6432,20 @@ void get_coordinates()
   for(int8_t i=0; i < NUM_AXIS; i++) {
     if(code_seen(axis_codes[i]))
     {
-      destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
+      bool relative = axis_relative_modes[i] || relative_mode;
+      destination[i] = (float)code_value();
+      if (i == E_AXIS) {
+        float emult = extruder_multiplier[active_extruder];
+        if (emult != 1.) {
+          if (! relative) {
+            destination[i] -= current_position[i];
+            relative = true;
+          }
+          destination[i] *= emult;
+        }
+      }
+      if (relative)
+        destination[i] += current_position[i];
       seen[i]=true;
     }
     else destination[i] = current_position[i]; //Are these else lines really needed?
@@ -6922,27 +6959,20 @@ void save_statistics(unsigned long _total_filament_used, unsigned long _total_pr
 
 }
 
-float calculate_volumetric_multiplier(float diameter) {
-	float area = .0;
-	float radius = .0;
-
-	radius = diameter * .5;
-	if (! volumetric_enabled || radius == 0) {
-		area = 1;
-	}
-	else {
-		area = M_PI * pow(radius, 2);
-	}
-
-	return 1.0 / area;
+float calculate_extruder_multiplier(float diameter) {
+  bool  enabled = volumetric_enabled && diameter > 0;
+  float area    = enabled ? (M_PI * pow(diameter * .5, 2)) : 0;
+	return (extrudemultiply == 100) ? 
+    (enabled ? (1.f / area) : 1.f) :
+    (enabled ? ((float(extrudemultiply) * 0.01f) / area) : 1.f);
 }
 
-void calculate_volumetric_multipliers() {
-	volumetric_multiplier[0] = calculate_volumetric_multiplier(filament_size[0]);
+void calculate_extruder_multipliers() {
+	extruder_multiplier[0] = calculate_extruder_multiplier(filament_size[0]);
 #if EXTRUDERS > 1
-	volumetric_multiplier[1] = calculate_volumetric_multiplier(filament_size[1]);
+	extruder_multiplier[1] = calculate_extruder_multiplier(filament_size[1]);
 #if EXTRUDERS > 2
-	volumetric_multiplier[2] = calculate_volumetric_multiplier(filament_size[2]);
+	extruder_multiplier[2] = calculate_extruder_multiplier(filament_size[2]);
 #endif
 #endif
 }
@@ -7474,7 +7504,7 @@ void uvlo_()
 
     // Read out the current Z motor microstep counter. This will be later used
     // for reaching the zero full step before powering off.
-    uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_TMC2130_CS);
+    uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
 
     // Calculate the file position, from which to resume this print.
     long sd_position = sdpos_atomic; //atomic sd position of last command added in queue
@@ -7566,7 +7596,7 @@ void uvlo_()
 
     st_synchronize();
     SERIAL_ECHOPGM("stps");
-    MYSERIAL.println(tmc2130_rd_MSCNT(Z_TMC2130_CS));
+    MYSERIAL.println(tmc2130_rd_MSCNT(Z_AXIS));
 
     disable_z();
     

Firmware/dogm_lcd_implementation.h

@@ -360,6 +360,15 @@ static void _drawmenu_setting_edit_generic(uint8_t row, const char* pstr, char p
 #define lcd_implementation_drawmenu_setting_edit_generic(row, pstr, pre_char, data) _drawmenu_setting_edit_generic(row, pstr, pre_char, data, false)
 #define lcd_implementation_drawmenu_setting_edit_generic_P(row, pstr, pre_char, data) _drawmenu_setting_edit_generic(row, pstr, pre_char, data, true)
 
+extern char *wfac_to_str5(const uint8_t &x);
+extern char *mres_to_str3(const uint8_t &x);
+
+#define lcd_implementation_drawmenu_setting_edit_wfac_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', wfac_to_str5(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_wfac(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', wfac_to_str5(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_mres_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', mres_to_str3(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_mres(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', mres_to_str3(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_byte3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', itostr3((uint8_t)*(data)))
+#define lcd_implementation_drawmenu_setting_edit_byte3(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', itostr3((uint8_t)*(data)))
 #define lcd_implementation_drawmenu_setting_edit_int3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_int3(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_float3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', ftostr3(*(data)))

Firmware/language_all.cpp

@@ -478,8 +478,8 @@ const char * const MSG_DATE_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_DATE_CZ
 };
 
-const char MSG_DEFAULT_SETTINGS_LOADED_EN[] PROGMEM = "Default settings loaded";
-const char MSG_DEFAULT_SETTINGS_LOADED_CZ[] PROGMEM = "Nahrano vychozi nastaveni";
+const char MSG_DEFAULT_SETTINGS_LOADED_EN[] PROGMEM = "Old settings found. Default PID, Esteps etc. will be set.";
+const char MSG_DEFAULT_SETTINGS_LOADED_CZ[] PROGMEM = "Neplatne hodnoty nastaveni. Bude pouzito vychozi PID, Esteps atd.";
 const char * const MSG_DEFAULT_SETTINGS_LOADED_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_DEFAULT_SETTINGS_LOADED_EN,
 	MSG_DEFAULT_SETTINGS_LOADED_CZ

Firmware/language_cz.h

@@ -375,7 +375,7 @@
 #define MSG_CHECK_IDLER						"Prosim otevrete idler a manualne odstrante filament."
 #define MSG_FILE_INCOMPLETE					"Soubor nekompletni. Pokracovat?"
 #define MSG_FILE_CNT						"Nektere soubory nebudou setrideny. Maximalni pocet souboru pro setrideni je 100."
-#define MSG_DEFAULT_SETTINGS_LOADED			"Nahrano vychozi nastaveni"
+#define MSG_DEFAULT_SETTINGS_LOADED			"Neplatne hodnoty nastaveni. Bude pouzito vychozi PID, Esteps atd."
 #define MSG_SORT_TIME						"Trideni     [Cas]"
 #define MSG_SORT_ALPHA						"Trideni [Abeceda]"
 #define MSG_SORT_NONE						"Trideni   [Zadne]"

Firmware/language_en.h

@@ -392,7 +392,7 @@
 #define(length=20, lines=4) MSG_PULL_OUT_FILAMENT				"Please pull out filament immediately"
 #define(length=20, lines=2) MSG_FILE_INCOMPLETE					"File incomplete. Continue anyway?"
 
-#define(length=20, lines=4) MSG_DEFAULT_SETTINGS_LOADED			"Default settings loaded"
+#define(length=20, lines=4) MSG_DEFAULT_SETTINGS_LOADED			"Old settings found. Default PID, Esteps etc. will be set."
 #define(length=17, lines=1) MSG_SORT_TIME						"Sort:      [Time]"
 #define(length=17, lines=1) MSG_SORT_ALPHA						"Sort:  [Alphabet]"
 #define(length=17, lines=1) MSG_SORT_NONE						"Sort:      [None]"

Firmware/planner.cpp

@@ -126,10 +126,6 @@ static uint8_t g_cntr_planner_queue_min = 0;
 float extrude_min_temp=EXTRUDE_MINTEMP;
 #endif
 
-#ifdef FILAMENT_SENSOR
- static char meas_sample; //temporary variable to hold filament measurement sample
-#endif
-
 #ifdef LIN_ADVANCE
     float extruder_advance_k = LIN_ADVANCE_K,
     advance_ed_ratio = LIN_ADVANCE_E_D_RATIO,
@@ -786,10 +782,6 @@ block->steps_y.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-p
 #endif
   block->steps_z.wide = labs(target[Z_AXIS]-position[Z_AXIS]);
   block->steps_e.wide = labs(target[E_AXIS]-position[E_AXIS]);
-  if (volumetric_multiplier[active_extruder] != 1.f)
-    block->steps_e.wide *= volumetric_multiplier[active_extruder];
-  if (extrudemultiply != 100)
-    block->steps_e.wide *= extrudemultiply * 0.01;
   block->step_event_count.wide = max(block->steps_x.wide, max(block->steps_y.wide, max(block->steps_z.wide, block->steps_e.wide)));
 
   // Bail if this is a zero-length block
@@ -919,7 +911,7 @@ Having the real displacement of the head, we can calculate the total movement le
     delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
   #endif
   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
-  delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
+  delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
   if ( block->steps_x.wide <=dropsegments && block->steps_y.wide <=dropsegments && block->steps_z.wide <=dropsegments )
   {
     block->millimeters = fabs(delta_mm[E_AXIS]);
@@ -955,49 +947,6 @@ Having the real displacement of the head, we can calculate the total movement le
   block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
   block->nominal_rate = ceil(block->step_event_count.wide * inverse_second); // (step/sec) Always > 0
 
-#ifdef FILAMENT_SENSOR
-  //FMM update ring buffer used for delay with filament measurements
-  
-  
-    if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1))  //only for extruder with filament sensor and if ring buffer is initialized
-  	  {
-    delay_dist = delay_dist + delta_mm[E_AXIS];  //increment counter with next move in e axis
-  
-    while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1)))  //check if counter is over max buffer size in mm
-      	  delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1);  //loop around the buffer
-    while (delay_dist<0)
-    	  delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
-      
-    delay_index1=delay_dist/10.0;  //calculate index
-    
-    //ensure the number is within range of the array after converting from floating point
-    if(delay_index1<0)
-    	delay_index1=0;
-    else if (delay_index1>MAX_MEASUREMENT_DELAY)
-    	delay_index1=MAX_MEASUREMENT_DELAY;
-    	
-    if(delay_index1 != delay_index2)  //moved index
-  	  {
-    	meas_sample=widthFil_to_size_ratio()-100;  //subtract off 100 to reduce magnitude - to store in a signed char
-  	  }
-    while( delay_index1 != delay_index2)
-  	  {
-  	  delay_index2 = delay_index2 + 1;
-  	if(delay_index2>MAX_MEASUREMENT_DELAY)
-  			  delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1);  //loop around buffer when incrementing
-  	  if(delay_index2<0)
-  		delay_index2=0;
-  	  else if (delay_index2>MAX_MEASUREMENT_DELAY)
-  		delay_index2=MAX_MEASUREMENT_DELAY;  
-  	  
-  	  measurement_delay[delay_index2]=meas_sample;
-  	  }
-    	
-    
-  	  }
-#endif
-
-
   // Calculate and limit speed in mm/sec for each axis
   float current_speed[4];
   float speed_factor = 1.0; //factor <=1 do decrease speed

Firmware/planner.h

@@ -170,6 +170,8 @@ extern float max_jerk[NUM_AXIS];
 extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
+extern long position[NUM_AXIS];
+
 #ifdef AUTOTEMP
     extern bool autotemp_enabled;
     extern float autotemp_max;

Firmware/stepper.cpp

@@ -1521,6 +1521,9 @@ void microstep_init()
   #endif
 }
 
+
+#ifndef TMC2130
+
 void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
 {
   if(ms1 > -1) switch(driver)
@@ -1578,3 +1581,4 @@ void microstep_readings()
       SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
       #endif
 }
+#endif //TMC2130

Firmware/temperature.cpp

@@ -104,9 +104,6 @@ unsigned char soft_pwm_bed;
   volatile int babystepsTodo[3]={0,0,0};
 #endif
 
-#ifdef FILAMENT_SENSOR
-  int current_raw_filwidth = 0;  //Holds measured filament diameter - one extruder only
-#endif  
 //===========================================================================
 //=============================private variables============================
 //===========================================================================
@@ -204,9 +201,6 @@ unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
 #define SOFT_PWM_SCALE 0
 #endif
 
-#ifdef FILAMENT_SENSOR
-  static int meas_shift_index;  //used to point to a delayed sample in buffer for filament width sensor
-#endif
 //===========================================================================
 //=============================   functions      ============================
 //===========================================================================
@@ -794,27 +788,6 @@ void manage_heater()
     #endif
   #endif
   
-//code for controlling the extruder rate based on the width sensor 
-#ifdef FILAMENT_SENSOR
-  if(filament_sensor) 
-	{
-	meas_shift_index=delay_index1-meas_delay_cm;
-		  if(meas_shift_index<0)
-			  meas_shift_index = meas_shift_index + (MAX_MEASUREMENT_DELAY+1);  //loop around buffer if needed
-		  
-		  //get the delayed info and add 100 to reconstitute to a percent of the nominal filament diameter
-		  //then square it to get an area
-		  
-		  if(meas_shift_index<0)
-			  meas_shift_index=0;
-		  else if (meas_shift_index>MAX_MEASUREMENT_DELAY)
-			  meas_shift_index=MAX_MEASUREMENT_DELAY;
-		  
-		     volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/100.0,2);
-		     if (volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] <0.01)
-		    	 volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]=0.01;
-	}
-#endif
 #ifdef HOST_KEEPALIVE_FEATURE
   host_keepalive();
 #endif
@@ -967,9 +940,7 @@ static void updateTemperaturesFromRawValues()
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
       redundant_temperature = analog2temp(redundant_temperature_raw, 1);
     #endif
-    #if defined (FILAMENT_SENSOR) && (FILWIDTH_PIN > -1)    //check if a sensor is supported 
-      filament_width_meas = analog2widthFil();
-    #endif  
+
     //Reset the watchdog after we know we have a temperature measurement.
     watchdog_reset();
 
@@ -979,35 +950,6 @@ static void updateTemperaturesFromRawValues()
 }
 
 
-// For converting raw Filament Width to milimeters 
-#ifdef FILAMENT_SENSOR
-float analog2widthFil() { 
-return current_raw_filwidth/16383.0*5.0; 
-//return current_raw_filwidth; 
-} 
- 
-// For converting raw Filament Width to a ratio 
-int widthFil_to_size_ratio() { 
- 
-float temp; 
-      
-temp=filament_width_meas;
-if(filament_width_meas<MEASURED_LOWER_LIMIT)
-	temp=filament_width_nominal;  //assume sensor cut out
-else if (filament_width_meas>MEASURED_UPPER_LIMIT)
-	temp= MEASURED_UPPER_LIMIT;
-
-
-return(filament_width_nominal/temp*100); 
-
-
-} 
-#endif
-
-
-
-
-
 void tp_init()
 {
 #if MB(RUMBA) && ((TEMP_SENSOR_0==-1)||(TEMP_SENSOR_1==-1)||(TEMP_SENSOR_2==-1)||(TEMP_SENSOR_BED==-1))

Firmware/temperature.h

@@ -31,14 +31,6 @@
 void tp_init();  //initialize the heating
 void manage_heater(); //it is critical that this is called periodically.
 
-#ifdef FILAMENT_SENSOR
-// For converting raw Filament Width to milimeters 
- float analog2widthFil(); 
- 
-// For converting raw Filament Width to an extrusion ratio 
- int widthFil_to_size_ratio();
-#endif
-
 // low level conversion routines
 // do not use these routines and variables outside of temperature.cpp
 extern int target_temperature[EXTRUDERS];  

Firmware/tmc2130.cpp

@@ -20,8 +20,6 @@ extern long st_get_position(uint8_t axis);
 extern void crashdet_stop_and_save_print();
 extern void crashdet_stop_and_save_print2();
 
-//chipselect pins
-uint8_t tmc2130_cs[4] = { X_TMC2130_CS, Y_TMC2130_CS, Z_TMC2130_CS, E0_TMC2130_CS };
 //mode
 uint8_t tmc2130_mode = TMC2130_MODE_NORMAL;
 //holding currents
@@ -30,7 +28,7 @@ uint8_t tmc2130_current_h[4] = TMC2130_CURRENTS_H;
 uint8_t tmc2130_current_r[4] = TMC2130_CURRENTS_R;
 
 //running currents for homing
-uint8_t tmc2130_current_r_home[4] = {10, 10, 20, 10};
+uint8_t tmc2130_current_r_home[4] = {8, 10, 20, 18};
 
 
 //pwm_ampl
@@ -55,6 +53,12 @@ uint8_t tmc2130_sg_meassure = 0xff;
 uint16_t tmc2130_sg_meassure_cnt = 0;
 uint32_t tmc2130_sg_meassure_val = 0;
 
+uint8_t tmc2130_home_enabled = 0;
+uint8_t tmc2130_home_origin[2] = {0, 0};
+uint8_t tmc2130_home_bsteps[2] = {48, 48};
+uint8_t tmc2130_home_fsteps[2] = {48, 48};
+
+uint8_t tmc2130_wave_fac[4] = {0, 0, 0, 0};
 
 bool tmc2130_sg_stop_on_crash = true;
 uint8_t tmc2130_sg_diag_mask = 0x00;
@@ -104,21 +108,19 @@ bool skip_debug_msg = false;
 #define TMC2130_REG_LOST_STEPS 0x73 // 20 bits
 
 
-uint16_t tmc2130_rd_TSTEP(uint8_t cs);
-uint16_t tmc2130_rd_MSCNT(uint8_t cs);
-uint16_t tmc2130_rd_DRV_STATUS(uint8_t cs);
+uint16_t tmc2130_rd_TSTEP(uint8_t axis);
+uint16_t tmc2130_rd_MSCNT(uint8_t axis);
+uint32_t tmc2130_rd_MSCURACT(uint8_t axis);
 
-void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
-void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
-void tmc2130_wr_TPWMTHRS(uint8_t cs, uint32_t val32);
-void tmc2130_wr_THIGH(uint8_t cs, uint32_t val32);
+void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
+void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
+void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32);
+void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32);
 
-uint8_t tmc2130_axis_by_cs(uint8_t cs);
-uint8_t tmc2130_calc_mres(uint16_t microstep_resolution);
 
-uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval);
-uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval);
-uint8_t tmc2130_txrx(uint8_t cs, uint8_t addr, uint32_t wval, uint32_t* rval);
+uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval);
+uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval);
+uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval);
 
 
 void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r);
@@ -128,10 +130,10 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
 void tmc2130_init()
 {
 	DBG(_n("tmc2130_init(), mode=%S\n"), tmc2130_mode?_n("STEALTH"):_n("NORMAL"));
-	tmc2130_mres[0] = tmc2130_calc_mres(TMC2130_USTEPS_XY);
-	tmc2130_mres[1] = tmc2130_calc_mres(TMC2130_USTEPS_XY);
-	tmc2130_mres[2] = tmc2130_calc_mres(TMC2130_USTEPS_Z);
-	tmc2130_mres[3] = tmc2130_calc_mres(TMC2130_USTEPS_E);
+/*	tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
+	tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);*/
 	WRITE(X_TMC2130_CS, HIGH);
 	WRITE(Y_TMC2130_CS, HIGH);
 	WRITE(Z_TMC2130_CS, HIGH);
@@ -147,65 +149,33 @@ void tmc2130_init()
 	SPI.begin();
 	for (int axis = 0; axis < 2; axis++) // X Y axes
 	{
-/*		if (tmc2130_current_r[axis] <= 31)
-		{
-			tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
-			tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
-		}
-		else
-		{
-			tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
-			tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((tmc2130_current_r[axis] >> 1) & 0x1f) << 8) | ((tmc2130_current_h[axis] >> 1) & 0x1f));
-		}*/
 		tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
-
-//		tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_XY, 0, 0);
-//		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
-//		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
-		tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
-		tmc2130_wr_TPWMTHRS(tmc2130_cs[axis], TMC2130_TPWMTHRS);
-		//tmc2130_wr_THIGH(tmc2130_cs[axis], TMC2130_THIGH);
+		tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
+		tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
+		tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
+		tmc2130_wr(axis, TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
+		tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
+		tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
+		//tmc2130_wr_THIGH(axis, TMC2130_THIGH);
 	}
 	for (int axis = 2; axis < 3; axis++) // Z axis
 	{
-//		uint8_t mres = tmc2130_mres(TMC2130_USTEPS_Z);
-/*		if (tmc2130_current_r[axis] <= 31)
-		{
-			tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_Z, 0, 0);
-			tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
-		}
-		else
-		{
-			tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, mres, TMC2130_INTPOL_Z, 0, 0);
-			tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((tmc2130_current_r[axis] >> 1) & 0x1f) << 8) | ((tmc2130_current_h[axis] >> 1) & 0x1f));
-		}*/
 		tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
-
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
-
+		tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
+		tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
 	}
 	for (int axis = 3; axis < 4; axis++) // E axis
 	{
-//		uint8_t mres = tmc2130_mres(TMC2130_USTEPS_E);
 		tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
-
-//		tmc2130_wr_CHOPCONF(tmc2130_cs[axis], 3, 5, 1, 0, 0, 0, 0, 2, 1, 0, 0, 0, mres, TMC2130_INTPOL_E, 0, 0);
-//		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((tmc2130_current_r[axis] & 0x1f) << 8) | (tmc2130_current_h[axis] & 0x1f));
-
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TPOWERDOWN, 0x00000000);
+		tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
 #ifndef TMC2130_STEALTH_E
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
+		tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
 #else //TMC2130_STEALTH_E
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, 0);
-		tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SILENT);
-		tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
-		tmc2130_wr_TPWMTHRS(tmc2130_cs[axis], TMC2130_TPWMTHRS);
+		tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
+		tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
+		tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT);
+		tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
+		tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
 #endif //TMC2130_STEALTH_E
 	}
 
@@ -217,6 +187,11 @@ void tmc2130_init()
 	tmc2130_sg_cnt[1] = 0;
 	tmc2130_sg_cnt[2] = 0;
 	tmc2130_sg_cnt[3] = 0;
+
+	tmc2130_set_wave(X_AXIS, 247, tmc2130_wave_fac[X_AXIS]);
+	tmc2130_set_wave(Y_AXIS, 247, tmc2130_wave_fac[Y_AXIS]);
+	tmc2130_set_wave(Z_AXIS, 247, tmc2130_wave_fac[Z_AXIS]);
+	tmc2130_set_wave(E_AXIS, 247, tmc2130_wave_fac[E_AXIS]);
 }
 
 uint8_t tmc2130_sample_diag()
@@ -279,11 +254,10 @@ bool tmc2130_update_sg()
 {
 	if (tmc2130_sg_meassure <= E_AXIS)
 	{
-		uint8_t cs = tmc2130_cs[tmc2130_sg_meassure];
-		uint16_t sg = tmc2130_rd_DRV_STATUS(cs) & 0x3ff;
-		tmc2130_sg_meassure_val += sg;
+		uint32_t val32 = 0;
+		tmc2130_rd(tmc2130_sg_meassure, TMC2130_REG_DRV_STATUS, &val32);
+		tmc2130_sg_meassure_val += (val32 & 0x3ff);
 		tmc2130_sg_meassure_cnt++;
-//		printf_P(PSTR("tmc2130_update_sg - meassure - sg=%d\n"), sg);
 		return true;
 	}
 	return false;
@@ -296,18 +270,17 @@ void tmc2130_home_enter(uint8_t axes_mask)
 	for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
 	{
 		uint8_t mask = (X_AXIS_MASK << axis);
-		uint8_t cs = tmc2130_cs[axis];
 		if (axes_mask & mask)
 		{
 			sg_homing_axes_mask |= mask;
 			//Configuration to spreadCycle
-			tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
-			tmc2130_wr(cs, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
-//			tmc2130_wr(cs, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
-			tmc2130_wr(cs, TMC2130_REG_TCOOLTHRS, (axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y);
+			tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
+			tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
+//			tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
+			tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y);
 			tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r_home[axis]);
 			if (mask & (X_AXIS_MASK | Y_AXIS_MASK | Z_AXIS_MASK))
-				tmc2130_wr(cs, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
+				tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
 		}
 	}
 #endif //TMC2130_SG_HOMING
@@ -326,18 +299,18 @@ void tmc2130_home_exit()
 			{
 				if (tmc2130_mode == TMC2130_MODE_SILENT)
 				{
-					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
-					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, 0);
-//					tmc2130_wr_PWMCONF(tmc2130_cs[i], tmc2130_pwm_ampl[i], tmc2130_pwm_grad[i], tmc2130_pwm_freq[i], tmc2130_pwm_auto[i], 0, 0);
+					tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
+					tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
+//					tmc2130_wr_PWMCONF(i, tmc2130_pwm_ampl[i], tmc2130_pwm_grad[i], tmc2130_pwm_freq[i], tmc2130_pwm_auto[i], 0, 0);
 				}
 				else
 				{
-//					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
+//					tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
 					tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
-//					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
-					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
-					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
-					tmc2130_wr(tmc2130_cs[axis], TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
+//					tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
+					tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
+					tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:((axis==X_AXIS)?TMC2130_TCOOLTHRS_X:TMC2130_TCOOLTHRS_Y));
+					tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
 				}
 			}
 		}
@@ -369,8 +342,8 @@ bool tmc2130_wait_standstill_xy(int timeout)
 	{
 		uint32_t drv_status_x = 0;
 		uint32_t drv_status_y = 0;
-		tmc2130_rd(tmc2130_cs[X_AXIS], TMC2130_REG_DRV_STATUS, &drv_status_x);
-		tmc2130_rd(tmc2130_cs[Y_AXIS], TMC2130_REG_DRV_STATUS, &drv_status_y);
+		tmc2130_rd(X_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_x);
+		tmc2130_rd(Y_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_y);
 //		DBG(_n("\tdrv_status_x=0x%08x drv_status_x=0x%08x\n"), drv_status_x, drv_status_y);
 		standstill = (drv_status_x & 0x80000000) && (drv_status_y & 0x80000000);
 		tmc2130_check_overtemp();
@@ -390,13 +363,13 @@ void tmc2130_check_overtemp()
 		{
 			uint32_t drv_status = 0;
 			skip_debug_msg = true;
-			tmc2130_rd(tmc2130_cs[i], TMC2130_REG_DRV_STATUS, &drv_status);
+			tmc2130_rd(i, TMC2130_REG_DRV_STATUS, &drv_status);
 			if (drv_status & ((uint32_t)1 << 26))
 			{ // BIT 26 - over temp prewarning ~120C (+-20C)
 				SERIAL_ERRORRPGM(TMC_OVERTEMP_MSG);
 				SERIAL_ECHOLN(i);
 				for (int j = 0; j < 4; j++)
-					tmc2130_wr(tmc2130_cs[j], TMC2130_REG_CHOPCONF, 0x00010000);
+					tmc2130_wr(j, TMC2130_REG_CHOPCONF, 0x00010000);
 				kill(TMC_OVERTEMP_MSG);
 			}
 
@@ -420,7 +393,6 @@ void tmc2130_check_overtemp()
 
 void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r)
 {
-	uint8_t cs = tmc2130_cs[axis];
 	uint8_t intpol = 1;
 	uint8_t toff = TMC2130_TOFF_XYZ; // toff = 3 (fchop = 27.778kHz)
 	uint8_t hstrt = 5; //initial 4, modified to 5
@@ -443,13 +415,13 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
 	}
 	if (current_r <= 31)
 	{
-		tmc2130_wr_CHOPCONF(cs, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 1, 0, 0, 0, mres, intpol, 0, 0);
-		tmc2130_wr(cs, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((current_r & 0x1f) << 8) | (current_h & 0x1f));
+		tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 1, 0, 0, 0, mres, intpol, 0, 0);
+		tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((current_r & 0x1f) << 8) | (current_h & 0x1f));
 	}
 	else
 	{
-		tmc2130_wr_CHOPCONF(cs, toff, hstrt, hend, fd3, 0, 0, 0, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
-		tmc2130_wr(cs, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
+		tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, 0, 0, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
+		tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
 	}
 }
 
@@ -485,7 +457,7 @@ void tmc2130_set_pwm_ampl(uint8_t axis, uint8_t pwm_ampl)
 	MYSERIAL.println((int)pwm_ampl);
 	tmc2130_pwm_ampl[axis] = pwm_ampl;
 	if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
-		tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
+		tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
 }
 
 void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_grad)
@@ -496,32 +468,61 @@ void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_grad)
 	MYSERIAL.println((int)pwm_grad);
 	tmc2130_pwm_grad[axis] = pwm_grad;
 	if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
-		tmc2130_wr_PWMCONF(tmc2130_cs[axis], tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
+		tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
 }
 
-uint16_t tmc2130_rd_TSTEP(uint8_t cs)
+uint16_t tmc2130_rd_TSTEP(uint8_t axis)
 {
 	uint32_t val32 = 0;
-	tmc2130_rd(cs, TMC2130_REG_TSTEP, &val32);
+	tmc2130_rd(axis, TMC2130_REG_TSTEP, &val32);
 	if (val32 & 0x000f0000) return 0xffff;
 	return val32 & 0xffff;
 }
 
-uint16_t tmc2130_rd_MSCNT(uint8_t cs)
+uint16_t tmc2130_rd_MSCNT(uint8_t axis)
 {
 	uint32_t val32 = 0;
-	tmc2130_rd(cs, TMC2130_REG_MSCNT, &val32);
+	tmc2130_rd(axis, TMC2130_REG_MSCNT, &val32);
 	return val32 & 0x3ff;
 }
 
-uint16_t tmc2130_rd_DRV_STATUS(uint8_t cs)
+uint32_t tmc2130_rd_MSCURACT(uint8_t axis)
 {
 	uint32_t val32 = 0;
-	tmc2130_rd(cs, TMC2130_REG_DRV_STATUS, &val32);
+	tmc2130_rd(axis, TMC2130_REG_MSCURACT, &val32);
 	return val32;
 }
 
-void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
+void tmc2130_wr_MSLUTSTART(uint8_t axis, uint8_t start_sin, uint8_t start_sin90)
+{
+	uint32_t val = 0;
+	val |= (uint32_t)start_sin;
+	val |= ((uint32_t)start_sin90) << 16;
+	tmc2130_wr(axis, TMC2130_REG_MSLUTSTART, val);
+	//printf_P(PSTR("MSLUTSTART=%08lx (start_sin=%d start_sin90=%d)\n"), val, start_sin, start_sin90);
+}
+
+void tmc2130_wr_MSLUTSEL(uint8_t axis, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t w0, uint8_t w1, uint8_t w2, uint8_t w3)
+{
+	uint32_t val = 0;
+	val |= ((uint32_t)w0);
+	val |= ((uint32_t)w1) << 2;
+	val |= ((uint32_t)w2) << 4;
+	val |= ((uint32_t)w3) << 6;
+	val |= ((uint32_t)x1) << 8;
+	val |= ((uint32_t)x2) << 16;
+	val |= ((uint32_t)x3) << 24;
+	tmc2130_wr(axis, TMC2130_REG_MSLUTSEL, val);
+	//printf_P(PSTR("MSLUTSEL=%08lx (x1=%d x2=%d x3=%d w0=%d w1=%d w2=%d w3=%d)\n"), val, x1, x2, x3, w0, w1, w2, w3);
+}
+
+void tmc2130_wr_MSLUT(uint8_t axis, uint8_t i, uint32_t val)
+{
+	tmc2130_wr(axis, TMC2130_REG_MSLUT0 + (i & 7), val);
+	//printf_P(PSTR("MSLUT[%d]=%08lx\n"), i, val);
+}
+
+void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
 {
 	uint32_t val = 0;
 	val |= (uint32_t)(toff & 15);
@@ -540,11 +541,11 @@ void tmc2130_wr_CHOPCONF(uint8_t cs, uint8_t toff, uint8_t hstrt, uint8_t hend,
 	val |= (uint32_t)(intpol & 1) << 28;
 	val |= (uint32_t)(dedge & 1) << 29;
 	val |= (uint32_t)(diss2g & 1) << 30;
-	tmc2130_wr(cs, TMC2130_REG_CHOPCONF, val);
+	tmc2130_wr(axis, TMC2130_REG_CHOPCONF, val);
 }
 
-//void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t PWMautoScale, uint8_t PWMfreq, uint8_t PWMgrad, uint8_t PWMampl)
-void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel)
+//void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t PWMautoScale, uint8_t PWMfreq, uint8_t PWMgrad, uint8_t PWMampl)
+void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel)
 {
 	uint32_t val = 0;
 	val |= (uint32_t)(pwm_ampl & 255);
@@ -553,54 +554,32 @@ void tmc2130_wr_PWMCONF(uint8_t cs, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t
 	val |= (uint32_t)(pwm_auto & 1) << 18;
 	val |= (uint32_t)(pwm_symm & 1) << 19;
 	val |= (uint32_t)(freewheel & 3) << 20;
-	tmc2130_wr(cs, TMC2130_REG_PWMCONF, val);
-//	tmc2130_wr(cs, TMC2130_REG_PWMCONF, ((uint32_t)(PWMautoScale+PWMfreq) << 16) | ((uint32_t)PWMgrad << 8) | PWMampl); // TMC LJ -> For better readability changed to 0x00 and added PWMautoScale and PWMfreq
+	tmc2130_wr(axis, TMC2130_REG_PWMCONF, val);
+//	tmc2130_wr(axis, TMC2130_REG_PWMCONF, ((uint32_t)(PWMautoScale+PWMfreq) << 16) | ((uint32_t)PWMgrad << 8) | PWMampl); // TMC LJ -> For better readability changed to 0x00 and added PWMautoScale and PWMfreq
 }
 
-void tmc2130_wr_TPWMTHRS(uint8_t cs, uint32_t val32)
+void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32)
 {
-	tmc2130_wr(cs, TMC2130_REG_TPWMTHRS, val32);
+	tmc2130_wr(axis, TMC2130_REG_TPWMTHRS, val32);
 }
 
-void tmc2130_wr_THIGH(uint8_t cs, uint32_t val32)
+void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32)
 {
-	tmc2130_wr(cs, TMC2130_REG_THIGH, val32);
+	tmc2130_wr(axis, TMC2130_REG_THIGH, val32);
 }
 
-#if defined(TMC2130_DEBUG_RD) || defined(TMC2130_DEBUG_WR)
-uint8_t tmc2130_axis_by_cs(uint8_t cs)
+uint8_t tmc2130_usteps2mres(uint16_t usteps)
 {
-	switch (cs)
-	{
-	case X_TMC2130_CS: return 0;
-	case Y_TMC2130_CS: return 1;
-	case Z_TMC2130_CS: return 2;
-	case E0_TMC2130_CS: return 3;
-	}
-	return -1;
-}
-#endif //TMC2130_DEBUG
-
-uint8_t tmc2130_calc_mres(uint16_t microstep_resolution)
-{
-	if (microstep_resolution == 256) return 0b0000;
-	if (microstep_resolution == 128) return 0b0001;
-	if (microstep_resolution == 64)  return 0b0010;
-	if (microstep_resolution == 32)  return 0b0011;
-	if (microstep_resolution == 16)  return 0b0100;
-	if (microstep_resolution == 8)   return 0b0101;
-	if (microstep_resolution == 4)   return 0b0110;
-	if (microstep_resolution == 2)   return 0b0111;
-	if (microstep_resolution == 1)   return 0b1000;
-	return 0;
+	uint8_t mres = 8; while (mres && (usteps >>= 1)) mres--;
+	return mres;
 }
 
-uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval)
+uint8_t tmc2130_wr(uint8_t axis, uint8_t addr, uint32_t wval)
 {
-	uint8_t stat = tmc2130_txrx(cs, addr | 0x80, wval, 0);
+	uint8_t stat = tmc2130_txrx(axis, addr | 0x80, wval, 0);
 #ifdef TMC2130_DEBUG_WR
 	MYSERIAL.print("tmc2130_wr(");
-	MYSERIAL.print((unsigned char)tmc2130_axis_by_cs(cs), DEC);
+	MYSERIAL.print((unsigned char)axis, DEC);
 	MYSERIAL.print(", 0x");
 	MYSERIAL.print((unsigned char)addr, HEX);
 	MYSERIAL.print(", 0x");
@@ -611,16 +590,16 @@ uint8_t tmc2130_wr(uint8_t cs, uint8_t addr, uint32_t wval)
 	return stat;
 }
 
-uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval)
+uint8_t tmc2130_rd(uint8_t axis, uint8_t addr, uint32_t* rval)
 {
 	uint32_t val32 = 0;
-	uint8_t stat = tmc2130_txrx(cs, addr, 0x00000000, &val32);
+	uint8_t stat = tmc2130_txrx(axis, addr, 0x00000000, &val32);
 	if (rval != 0) *rval = val32;
 #ifdef TMC2130_DEBUG_RD
 	if (!skip_debug_msg)
 	{
 		MYSERIAL.print("tmc2130_rd(");
-		MYSERIAL.print((unsigned char)tmc2130_axis_by_cs(cs), DEC);
+		MYSERIAL.print((unsigned char)axis, DEC);
 		MYSERIAL.print(", 0x");
 		MYSERIAL.print((unsigned char)addr, HEX);
 		MYSERIAL.print(", 0x");
@@ -633,28 +612,50 @@ uint8_t tmc2130_rd(uint8_t cs, uint8_t addr, uint32_t* rval)
 	return stat;
 }
 
-uint8_t tmc2130_txrx(uint8_t cs, uint8_t addr, uint32_t wval, uint32_t* rval)
+inline void tmc2130_cs_low(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: WRITE(X_TMC2130_CS, LOW); break;
+	case Y_AXIS: WRITE(Y_TMC2130_CS, LOW); break;
+	case Z_AXIS: WRITE(Z_TMC2130_CS, LOW); break;
+	case E_AXIS: WRITE(E0_TMC2130_CS, LOW); break;
+	}
+}
+
+inline void tmc2130_cs_high(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: WRITE(X_TMC2130_CS, HIGH); break;
+	case Y_AXIS: WRITE(Y_TMC2130_CS, HIGH); break;
+	case Z_AXIS: WRITE(Z_TMC2130_CS, HIGH); break;
+	case E_AXIS: WRITE(E0_TMC2130_CS, HIGH); break;
+	}
+}
+
+uint8_t tmc2130_txrx(uint8_t axis, uint8_t addr, uint32_t wval, uint32_t* rval)
 {
 	//datagram1 - request
 	SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
-	digitalWrite(cs, LOW);
+	tmc2130_cs_low(axis);
 	SPI.transfer(addr); // address
 	SPI.transfer((wval >> 24) & 0xff); // MSB
 	SPI.transfer((wval >> 16) & 0xff);
 	SPI.transfer((wval >> 8) & 0xff);
 	SPI.transfer(wval & 0xff); // LSB
-	digitalWrite(cs, HIGH);
+	tmc2130_cs_high(axis);
 	SPI.endTransaction();
 	//datagram2 - response
 	SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
-	digitalWrite(cs, LOW);
+	tmc2130_cs_low(axis);
 	uint8_t stat = SPI.transfer(0); // status
 	uint32_t val32 = 0;
 	val32 = SPI.transfer(0); // MSB
 	val32 = (val32 << 8) | SPI.transfer(0);
 	val32 = (val32 << 8) | SPI.transfer(0);
 	val32 = (val32 << 8) | SPI.transfer(0); // LSB
-	digitalWrite(cs, HIGH);
+	tmc2130_cs_high(axis);
 	SPI.endTransaction();
 	if (rval != 0) *rval = val32;
 	return stat;
@@ -669,5 +670,461 @@ void tmc2130_eeprom_save_config()
 
 }
 
+#define _GET_PWR_X      (READ(X_ENABLE_PIN) == X_ENABLE_ON)
+#define _GET_PWR_Y      (READ(Y_ENABLE_PIN) == Y_ENABLE_ON)
+#define _GET_PWR_Z      (READ(Z_ENABLE_PIN) == Z_ENABLE_ON)
+#define _GET_PWR_E      (READ(E0_ENABLE_PIN) == E_ENABLE_ON)
+
+#define _SET_PWR_X(ena) { WRITE(X_ENABLE_PIN, ena?X_ENABLE_ON:!X_ENABLE_ON); asm("nop"); }
+#define _SET_PWR_Y(ena) { WRITE(Y_ENABLE_PIN, ena?Y_ENABLE_ON:!Y_ENABLE_ON); asm("nop"); }
+#define _SET_PWR_Z(ena) { WRITE(Z_ENABLE_PIN, ena?Z_ENABLE_ON:!Z_ENABLE_ON); asm("nop"); }
+#define _SET_PWR_E(ena) { WRITE(E0_ENABLE_PIN, ena?E_ENABLE_ON:!E_ENABLE_ON); asm("nop"); }
+
+#define _GET_DIR_X      (READ(X_DIR_PIN) == INVERT_X_DIR)
+#define _GET_DIR_Y      (READ(Y_DIR_PIN) == INVERT_Y_DIR)
+#define _GET_DIR_Z      (READ(Z_DIR_PIN) == INVERT_Z_DIR)
+#define _GET_DIR_E      (READ(E0_DIR_PIN) == INVERT_E0_DIR)
+
+#define _SET_DIR_X(dir) { WRITE(X_DIR_PIN, dir?INVERT_X_DIR:!INVERT_X_DIR); asm("nop"); }
+#define _SET_DIR_Y(dir) { WRITE(Y_DIR_PIN, dir?INVERT_Y_DIR:!INVERT_Y_DIR); asm("nop"); }
+#define _SET_DIR_Z(dir) { WRITE(Z_DIR_PIN, dir?INVERT_Z_DIR:!INVERT_Z_DIR); asm("nop"); }
+#define _SET_DIR_E(dir) { WRITE(E0_DIR_PIN, dir?INVERT_E0_DIR:!INVERT_E0_DIR); asm("nop"); }
+
+#define _DO_STEP_X      { WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); asm("nop"); WRITE(X_STEP_PIN, INVERT_X_STEP_PIN); asm("nop"); }
+#define _DO_STEP_Y      { WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN); asm("nop"); WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN); asm("nop"); }
+#define _DO_STEP_Z      { WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN); asm("nop"); WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN); asm("nop"); }
+#define _DO_STEP_E      { WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); asm("nop"); WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); asm("nop"); }
+
+
+uint16_t tmc2130_get_res(uint8_t axis)
+{
+	return tmc2130_mres2usteps(tmc2130_mres[axis]);
+}
+
+void tmc2130_set_res(uint8_t axis, uint16_t res)
+{
+	tmc2130_mres[axis] = tmc2130_usteps2mres(res);
+//	uint32_t u = micros();
+	tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
+//	u = micros() - u;
+//	printf_P(PSTR("tmc2130_setup_chopper %c %lu us"), "XYZE"[axis], u);
+}
+
+uint8_t tmc2130_get_pwr(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: return _GET_PWR_X;
+	case Y_AXIS: return _GET_PWR_Y;
+	case Z_AXIS: return _GET_PWR_Z;
+	case E_AXIS: return _GET_PWR_E;
+	}
+	return 0;
+}
+
+void tmc2130_set_pwr(uint8_t axis, uint8_t pwr)
+{
+	switch (axis)
+	{
+	case X_AXIS: _SET_PWR_X(pwr); break;
+	case Y_AXIS: _SET_PWR_Y(pwr); break;
+	case Z_AXIS: _SET_PWR_Z(pwr); break;
+	case E_AXIS: _SET_PWR_E(pwr); break;
+	}
+}
+
+uint8_t tmc2130_get_inv(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: return INVERT_X_DIR;
+	case Y_AXIS: return INVERT_Y_DIR;
+	case Z_AXIS: return INVERT_Z_DIR;
+	case E_AXIS: return INVERT_E0_DIR;
+	}
+	return 0;
+}
+
+uint8_t tmc2130_get_dir(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: return _GET_DIR_X;
+	case Y_AXIS: return _GET_DIR_Y;
+	case Z_AXIS: return _GET_DIR_Z;
+	case E_AXIS: return _GET_DIR_E;
+	}
+	return 0;
+}
+
+
+void tmc2130_set_dir(uint8_t axis, uint8_t dir)
+{
+	switch (axis)
+	{
+	case X_AXIS: _SET_DIR_X(dir); break;
+	case Y_AXIS: _SET_DIR_Y(dir); break;
+	case Z_AXIS: _SET_DIR_Z(dir); break;
+	case E_AXIS: _SET_DIR_E(dir); break;
+	}
+}
+
+void tmc2130_do_step(uint8_t axis)
+{
+	switch (axis)
+	{
+	case X_AXIS: _DO_STEP_X; break;
+	case Y_AXIS: _DO_STEP_Y; break;
+	case Z_AXIS: _DO_STEP_Z; break;
+	case E_AXIS: _DO_STEP_E; break;
+	}
+}
+
+void tmc2130_do_steps(uint8_t axis, uint16_t steps, uint8_t dir, uint16_t delay_us)
+{
+	tmc2130_set_dir(axis, dir);
+	delayMicroseconds(100);
+	while (steps--)
+	{
+		tmc2130_do_step(axis);
+		delayMicroseconds(delay_us);
+	}
+}
+
+void tmc2130_goto_step(uint8_t axis, uint8_t step, uint8_t dir, uint16_t delay_us, uint16_t microstep_resolution)
+{
+	printf_P(PSTR("tmc2130_goto_step %d %d %d %d \n"), axis, step, dir, delay_us, microstep_resolution);
+	uint8_t shift; for (shift = 0; shift < 8; shift++) if (microstep_resolution == (256 >> shift)) break;
+	uint16_t cnt = 4 * (1 << (8 - shift));
+	uint16_t mscnt = tmc2130_rd_MSCNT(axis);
+	if (dir == 2)
+	{
+		dir = tmc2130_get_inv(axis)?0:1;
+		int steps = (int)step - (int)(mscnt >> shift);
+		if (steps < 0)
+		{
+			dir ^= 1;
+			steps = -steps;
+		}
+		if (steps > (cnt / 2))
+		{
+			dir ^= 1;
+			steps = cnt - steps;
+		}
+		cnt = steps;
+	}
+	tmc2130_set_dir(axis, dir);
+	delayMicroseconds(100);
+	mscnt = tmc2130_rd_MSCNT(axis);
+	while ((cnt--) && ((mscnt >> shift) != step))
+	{
+		tmc2130_do_step(axis);
+		delayMicroseconds(delay_us);
+		mscnt = tmc2130_rd_MSCNT(axis);
+	}
+}
+
+void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream)
+{
+	uint8_t pwr = tmc2130_get_pwr(axis);
+	tmc2130_set_pwr(axis, 0);
+	tmc2130_setup_chopper(axis, tmc2130_usteps2mres(256), tmc2130_current_h[axis], tmc2130_current_r[axis]);
+	tmc2130_goto_step(axis, 0, 2, 100, 256);
+	tmc2130_set_dir(axis, tmc2130_get_inv(axis)?0:1);
+	for (int i = 0; i <= 255; i++)
+	{
+		uint32_t val = tmc2130_rd_MSCURACT(axis);
+		uint16_t mscnt = tmc2130_rd_MSCNT(axis);
+		int curA = (val & 0xff) | ((val << 7) & 0x8000);
+		if (stream)
+		{
+			if (mscnt == i)
+				fprintf_P(stream, PSTR("%d\t%d\n"), i, curA);
+			else //TODO - remove this check
+				fprintf_P(stream, PSTR("!! (i=%d MSCNT=%d)\n"), i, mscnt);
+		}
+		if (data) *(data++) = curA;
+		tmc2130_do_step(axis);
+		delayMicroseconds(100);
+	}
+	tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
+}
+
+void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200)
+{
+// TMC2130 wave compression algorithm
+// optimized for minimal memory requirements
+	printf_P(PSTR("tmc2130_set_wave %d %d\n"), axis, fac200);
+	if (fac200 < TMC2130_WAVE_FAC200_MIN) fac200 = 0;
+	if (fac200 > TMC2130_WAVE_FAC200_MAX) fac200 = TMC2130_WAVE_FAC200_MAX;
+	float fac = (float)fac200/200; //correction factor
+	uint8_t vA = 0;                //value of currentA
+	uint8_t va = 0;                //previous vA
+	uint8_t d0 = 0;                //delta0
+	uint8_t d1 = 1;                //delta1
+	uint8_t w[4] = {1,1,1,1};      //W bits (MSLUTSEL)
+	uint8_t x[3] = {255,255,255};  //X segment bounds (MSLUTSEL)
+	uint8_t s = 0;                 //current segment
+	int8_t b;                      //encoded bit value
+	uint8_t dA;                    //delta value
+	int i;                         //microstep index
+	uint32_t reg;                  //tmc2130 register
+	tmc2130_wr_MSLUTSTART(axis, 0, amp);
+	for (i = 0; i < 256; i++)
+	{
+		if ((i & 31) == 0)
+			reg = 0;
+		// calculate value
+		if (fac == 0) // default TMC wave
+			vA = (uint8_t)((amp+1) * sin((2*PI*i + PI)/1024) + 0.5) - 1;
+		else // corrected wave
+			vA = (uint8_t)(amp * pow(sin(2*PI*i/1024), fac) + 0.5);
+		dA = vA - va; // calculate delta
+		va = vA;
+		b = -1;
+		if (dA == d0) b = 0;      //delta == delta0 => bit=0
+		else if (dA == d1) b = 1; //delta == delta1 => bit=1
+		else
+		{
+			if (dA < d0) // delta < delta0 => switch wbit down
+			{
+				//printf("dn\n");
+				b = 0;
+				switch (dA)
+				{
+				case -1: d0 = -1; d1 = 0; w[s+1] = 0; break;
+				case  0: d0 =  0; d1 = 1; w[s+1] = 1; break;
+				case  1: d0 =  1; d1 = 2; w[s+1] = 2; break;
+				default: b = -1; break;
+				}
+				if (b >= 0) { x[s] = i; s++; }
+			}
+			else if (dA > d1) // delta > delta0 => switch wbit up
+			{
+				//printf("up\n");
+				b = 1;
+				switch (dA)
+				{
+				case  1: d0 =  0; d1 = 1; w[s+1] = 1; break;
+				case  2: d0 =  1; d1 = 2; w[s+1] = 2; break;
+				case  3: d0 =  2; d1 = 3; w[s+1] = 3; break;
+				default: b = -1; break;
+				}
+			    if (b >= 0) { x[s] = i; s++; }
+			}
+		}
+		if (b < 0) break; // delta out of range (<-1 or >3)
+		if (s > 3) break; // segment out of range (> 3)
+		//printf("%d\n", vA);
+		if (b == 1) reg |= 0x80000000;
+		if ((i & 31) == 31)
+			tmc2130_wr_MSLUT(axis, (uint8_t)(i >> 5), reg);
+		else
+			reg >>= 1;
+//		printf("%3d\t%3d\t%2d\t%2d\t%2d\t%2d    %08x\n", i, vA, dA, b, w[s], s, reg);
+	}
+	tmc2130_wr_MSLUTSEL(axis, x[0], x[1], x[2], w[0], w[1], w[2], w[3]);
+
+/*
+//	printf_P(PSTR(" tmc2130_set_wave %d %d\n"), axis, fac200);
+	switch (fac200)
+	{
+	case 0: //default TMC wave 247/0
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0xaaaab556);
+		tmc2130_wr_MSLUT(axis, 1, 0x4a9554aa);
+		tmc2130_wr_MSLUT(axis, 2, 0x24492929);
+		tmc2130_wr_MSLUT(axis, 3, 0x10104222);
+		tmc2130_wr_MSLUT(axis, 4, 0xf8000000);
+		tmc2130_wr_MSLUT(axis, 5, 0xb5bb777d);
+		tmc2130_wr_MSLUT(axis, 6, 0x49295556);
+		tmc2130_wr_MSLUT(axis, 7, 0x00404222);
+		tmc2130_wr_MSLUTSEL(axis, 2, 154, 255, 1, 2, 1, 1);
+		break;
+	case 210: //calculated wave 247/1.050
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x55294a4e);
+		tmc2130_wr_MSLUT(axis, 1, 0xa52a552a);
+		tmc2130_wr_MSLUT(axis, 2, 0x48949294);
+		tmc2130_wr_MSLUT(axis, 3, 0x81042222);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000000);
+		tmc2130_wr_MSLUT(axis, 5, 0xdb6eef7e);
+		tmc2130_wr_MSLUT(axis, 6, 0x9295555a);
+		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
+		tmc2130_wr_MSLUTSEL(axis, 3, 160, 255, 1, 2, 1, 1);
+		break;
+	case 212: //calculated wave 247/1.060
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x4a94948e);
+		tmc2130_wr_MSLUT(axis, 1, 0x94a952a5);
+		tmc2130_wr_MSLUT(axis, 2, 0x24925252);
+		tmc2130_wr_MSLUT(axis, 3, 0x10421112);
+		tmc2130_wr_MSLUT(axis, 4, 0xc0000020);
+		tmc2130_wr_MSLUT(axis, 5, 0xdb7777df);
+		tmc2130_wr_MSLUT(axis, 6, 0x9295556a);
+		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
+		tmc2130_wr_MSLUTSEL(axis, 3, 157, 255, 1, 2, 1, 1);
+		break;
+	case 214: //calculated wave 247/1.070
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0xa949489e);
+		tmc2130_wr_MSLUT(axis, 1, 0x52a54a54);
+		tmc2130_wr_MSLUT(axis, 2, 0x224a494a);
+		tmc2130_wr_MSLUT(axis, 3, 0x04108889);
+		tmc2130_wr_MSLUT(axis, 4, 0xffc08002);
+		tmc2130_wr_MSLUT(axis, 5, 0x6dbbbdfb);
+		tmc2130_wr_MSLUT(axis, 6, 0x94a555ab);
+		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
+		tmc2130_wr_MSLUTSEL(axis, 4, 149, 255, 1, 2, 1, 1);
+		break;
+	case 215: //calculated wave 247/1.075
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x4a52491e);
+		tmc2130_wr_MSLUT(axis, 1, 0xa54a54a9);
+		tmc2130_wr_MSLUT(axis, 2, 0x49249494);
+		tmc2130_wr_MSLUT(axis, 3, 0x10421122);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000008);
+		tmc2130_wr_MSLUT(axis, 5, 0x6ddbdefc);
+		tmc2130_wr_MSLUT(axis, 6, 0x94a555ad);
+		tmc2130_wr_MSLUT(axis, 7, 0x00408444);
+		tmc2130_wr_MSLUTSEL(axis, 4, 161, 255, 1, 2, 1, 1);
+		break;
+	case 216: //calculated wave 247/1.080
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x9494911e);
+		tmc2130_wr_MSLUT(axis, 1, 0x4a94a94a);
+		tmc2130_wr_MSLUT(axis, 2, 0x92492929);
+		tmc2130_wr_MSLUT(axis, 3, 0x41044444);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000040);
+		tmc2130_wr_MSLUT(axis, 5, 0xaedddf7f);
+		tmc2130_wr_MSLUT(axis, 6, 0x94a956ad);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
+		tmc2130_wr_MSLUTSEL(axis, 4, 159, 255, 1, 2, 1, 1);
+		break;
+	case 218: //calculated wave 247/1.090
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x4a49223e);
+		tmc2130_wr_MSLUT(axis, 1, 0x4a52a529);
+		tmc2130_wr_MSLUT(axis, 2, 0x49252529);
+		tmc2130_wr_MSLUT(axis, 3, 0x08422224);
+		tmc2130_wr_MSLUT(axis, 4, 0xfc008004);
+		tmc2130_wr_MSLUT(axis, 5, 0xb6eef7df);
+		tmc2130_wr_MSLUT(axis, 6, 0xa4aaaab5);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808448);
+		tmc2130_wr_MSLUTSEL(axis, 5, 153, 255, 1, 2, 1, 1);
+		break;
+	case 220: //calculated wave 247/1.100
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0xa492487e);
+		tmc2130_wr_MSLUT(axis, 1, 0x294a52a4);
+		tmc2130_wr_MSLUT(axis, 2, 0x492494a5);
+		tmc2130_wr_MSLUT(axis, 3, 0x82110912);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000080);
+		tmc2130_wr_MSLUT(axis, 5, 0xdb777df8);
+		tmc2130_wr_MSLUT(axis, 6, 0x252aaad6);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
+		tmc2130_wr_MSLUTSEL(axis, 6, 162, 255, 1, 2, 1, 1);
+		break;
+	case 222: //calculated wave 247/1.110
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x524910fe);
+		tmc2130_wr_MSLUT(axis, 1, 0xa5294a52);
+		tmc2130_wr_MSLUT(axis, 2, 0x24929294);
+		tmc2130_wr_MSLUT(axis, 3, 0x20844489);
+		tmc2130_wr_MSLUT(axis, 4, 0xc0004008);
+		tmc2130_wr_MSLUT(axis, 5, 0xdbbbdf7f);
+		tmc2130_wr_MSLUT(axis, 6, 0x252aab5a);
+		tmc2130_wr_MSLUT(axis, 7, 0x00808449);
+		tmc2130_wr_MSLUTSEL(axis, 7, 157, 255, 1, 2, 1, 1);
+		break;
+	case 224: //calculated wave 247/1.120
+		tmc2130_wr_MSLUTSTART(axis, 0, 247);
+		tmc2130_wr_MSLUT(axis, 0, 0x292223fe);
+		tmc2130_wr_MSLUT(axis, 1, 0x94a52949);
+		tmc2130_wr_MSLUT(axis, 2, 0x92524a52);
+		tmc2130_wr_MSLUT(axis, 3, 0x04222244);
+		tmc2130_wr_MSLUT(axis, 4, 0x00000101);
+		tmc2130_wr_MSLUT(axis, 5, 0x6dddefe0);
+		tmc2130_wr_MSLUT(axis, 6, 0x254aad5b);
+		tmc2130_wr_MSLUT(axis, 7, 0x00810889);
+		tmc2130_wr_MSLUTSEL(axis, 9, 164, 255, 1, 2, 1, 1);
+		break;
+	}*/
+}
+
+void bubblesort_uint8(uint8_t* data, uint8_t size, uint8_t* data2)
+{
+	uint8_t changed = 1;
+	while (changed)
+	{
+		changed = 0;
+		for (uint8_t i = 0; i < (size - 1); i++)
+			if (data[i] > data[i+1])
+			{
+				uint8_t register d = data[i];
+				data[i] = data[i+1];
+				data[i+1] = d;
+				if (data2)
+				{
+					d = data2[i];
+					data2[i] = data2[i+1];
+					data2[i+1] = d;
+				}
+				changed = 1;
+			}
+	}
+}
+
+uint8_t clusterize_uint8(uint8_t* data, uint8_t size, uint8_t* ccnt, uint8_t* cval, uint8_t tol)
+{
+	uint8_t cnt = 1;
+	uint16_t sum = data[0];
+	uint8_t cl = 0;
+	for (uint8_t i = 1; i < size; i++)
+	{
+		uint8_t d = data[i];
+		uint8_t val = sum / cnt;
+		uint8_t dif = 0;
+		if (val > d) dif = val - d;
+		else dif = d - val;
+		if (dif <= tol)
+		{
+			cnt += 1;
+			sum += d;
+		}
+		else
+		{
+			if (ccnt) ccnt[cl] = cnt;
+			if (cval) cval[cl] = val;
+			cnt = 1;
+			sum = d;
+			cl += 1;
+		}
+	}
+	if (ccnt) ccnt[cl] = cnt;
+	if (cval) cval[cl] = sum / cnt;
+	return ++cl;
+}
+
+void tmc2130_home_calibrate(uint8_t axis)
+{
+	uint8_t step[16];
+	uint8_t cnt[16];
+	uint8_t val[16];
+	homeaxis(axis, 16, step);
+	bubblesort_uint8(step, 16, 0);
+	printf_P(PSTR("sorted samples:\n"));
+	for (uint8_t i = 0; i < 16; i++)
+		printf_P(PSTR(" i=%2d step=%2d\n"), i, step[i]);
+	uint8_t cl = clusterize_uint8(step, 16, cnt, val, 1);
+	printf_P(PSTR("clusters:\n"));
+	for (uint8_t i = 0; i < cl; i++)
+		printf_P(PSTR(" i=%2d cnt=%2d val=%2d\n"), i, cnt[i], val[i]);
+	bubblesort_uint8(cnt, cl, val);
+	tmc2130_home_origin[axis] = val[cl-1];
+	printf_P(PSTR("result value: %d\n"), tmc2130_home_origin[axis]);
+	if (axis == X_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN, tmc2130_home_origin[X_AXIS]);
+	else if (axis == Y_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN, tmc2130_home_origin[Y_AXIS]);
+}
 
 #endif //TMC2130

Firmware/tmc2130.h

@@ -1,15 +1,16 @@
 #ifndef TMC2130_H
 #define TMC2130_H
 
-extern uint8_t tmc2130_cs[4];
 
 //mode
 extern uint8_t tmc2130_mode;
 //holding and running currents
 extern uint8_t tmc2130_current_h[4];
 extern uint8_t tmc2130_current_r[4];
-//flags for axis stall detection
+//microstep resolution (0 means 256usteps, 8 means 1ustep
+extern uint8_t tmc2130_mres[4];
 
+//flags for axis stall detection
 extern uint8_t tmc2130_sg_thr[4];
 
 extern bool tmc2130_sg_stop_on_crash;
@@ -22,6 +23,18 @@ extern uint32_t tmc2130_sg_meassure_val;
 #define TMC2130_MODE_NORMAL 0
 #define TMC2130_MODE_SILENT 1
 
+#define TMC2130_WAVE_FAC200_MIN 180
+#define TMC2130_WAVE_FAC200_MAX 250
+#define TMC2130_WAVE_FAC200_STP   1
+
+extern uint8_t tmc2130_home_enabled;
+extern uint8_t tmc2130_home_origin[2];
+extern uint8_t tmc2130_home_bsteps[2];
+extern uint8_t tmc2130_home_fsteps[2];
+
+extern uint8_t tmc2130_wave_fac[4];
+
+
 //initialize tmc2130
 extern void tmc2130_init();
 //check diag pins (called from stepper isr)
@@ -54,7 +67,11 @@ extern void tmc2130_set_pwm_ampl(uint8_t axis, uint8_t pwm_ampl);
 extern void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_ampl);
 
 
-extern uint16_t tmc2130_rd_MSCNT(uint8_t cs);
+extern uint16_t tmc2130_rd_MSCNT(uint8_t axis);
+extern uint32_t tmc2130_rd_MSCURACT(uint8_t axis);
+
+extern uint8_t tmc2130_usteps2mres(uint16_t usteps);
+#define tmc2130_mres2usteps(mres) ((uint16_t)256 >> mres)
 
 extern bool tmc2130_wait_standstill_xy(int timeout);
 
@@ -89,4 +106,19 @@ struct
 } tmc2130_axis_config;
 #pragma pack(pop)
 
+extern uint16_t tmc2130_get_res(uint8_t axis);
+extern void tmc2130_set_res(uint8_t axis, uint16_t res);
+extern uint8_t tmc2130_get_pwr(uint8_t axis);
+extern void tmc2130_set_pwr(uint8_t axis, uint8_t pwr);
+extern uint8_t tmc2130_get_inv(uint8_t axis);
+extern uint8_t tmc2130_get_dir(uint8_t axis);
+extern void tmc2130_set_dir(uint8_t axis, uint8_t dir);
+extern void tmc2130_do_step(uint8_t axis);
+extern void tmc2130_do_steps(uint8_t axis, uint16_t steps, uint8_t dir, uint16_t delay_us);
+extern void tmc2130_goto_step(uint8_t axis, uint8_t step, uint8_t dir, uint16_t delay_us, uint16_t microstep_resolution);
+extern void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream);
+extern void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac200);
+
+extern void tmc2130_home_calibrate(uint8_t axis);
+
 #endif //TMC2130_H

Firmware/ultralcd.cpp

@@ -228,6 +228,9 @@ static void menu_action_setlang(unsigned char lang);
 static void menu_action_sdfile(const char* filename, char* longFilename);
 static void menu_action_sddirectory(const char* filename, char* longFilename);
 static void menu_action_setting_edit_bool(const char* pstr, bool* ptr);
+static void menu_action_setting_edit_wfac(const char* pstr, uint8_t* ptr, uint8_t minValue, uint8_t maxValue);
+static void menu_action_setting_edit_mres(const char* pstr, uint8_t* ptr, uint8_t minValue, uint8_t maxValue);
+static void menu_action_setting_edit_byte3(const char* pstr, uint8_t* ptr, uint8_t minValue, uint8_t maxValue);
 static void menu_action_setting_edit_int3(const char* pstr, int* ptr, int minValue, int maxValue);
 static void menu_action_setting_edit_float3(const char* pstr, float* ptr, float minValue, float maxValue);
 static void menu_action_setting_edit_float32(const char* pstr, float* ptr, float minValue, float maxValue);
@@ -3946,6 +3949,219 @@ static void lcd_selftest_()
 	lcd_selftest();
 }
 
+
+static void lcd_experimantal_menu();
+static void lcd_homing_accuracy_menu();
+
+static void lcd_accurate_home_set()
+{
+	tmc2130_home_enabled = tmc2130_home_enabled?0:1;
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_ENABLED, tmc2130_home_enabled);
+}
+
+static void lcd_homing_accuracy_menu_advanced_reset()
+{
+	tmc2130_home_bsteps[X_AXIS] = 48;
+	tmc2130_home_fsteps[X_AXIS] = 48;
+	tmc2130_home_bsteps[Y_AXIS] = 48;
+	tmc2130_home_fsteps[Y_AXIS] = 48;
+}
+
+static void lcd_homing_accuracy_menu_advanced_save()
+{
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN, tmc2130_home_origin[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_BSTEPS, tmc2130_home_bsteps[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_FSTEPS, tmc2130_home_fsteps[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN, tmc2130_home_origin[Y_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_BSTEPS, tmc2130_home_bsteps[Y_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_FSTEPS, tmc2130_home_fsteps[Y_AXIS]);
+}
+
+static void lcd_homing_accuracy_menu_advanced_back()
+{
+	lcd_homing_accuracy_menu_advanced_save();
+	currentMenu = lcd_homing_accuracy_menu;
+	lcd_homing_accuracy_menu();
+}
+
+static void lcd_homing_accuracy_menu_advanced()
+{
+	lcd_timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
+	START_MENU();
+	MENU_ITEM(back, PSTR("Homing accuracy"), lcd_homing_accuracy_menu_advanced_back);
+	MENU_ITEM(function, PSTR("Reset def. steps"), lcd_homing_accuracy_menu_advanced_reset);
+	MENU_ITEM_EDIT(byte3, PSTR("X-origin"),  &tmc2130_home_origin[X_AXIS],  0, 63);
+	MENU_ITEM_EDIT(byte3, PSTR("Y-origin"),  &tmc2130_home_origin[Y_AXIS],  0, 63);
+	MENU_ITEM_EDIT(byte3, PSTR("X-bsteps"),  &tmc2130_home_bsteps[X_AXIS],  0, 128);
+	MENU_ITEM_EDIT(byte3, PSTR("Y-bsteps"),  &tmc2130_home_bsteps[Y_AXIS],  0, 128);
+	MENU_ITEM_EDIT(byte3, PSTR("X-fsteps"),  &tmc2130_home_fsteps[X_AXIS],  0, 128);
+	MENU_ITEM_EDIT(byte3, PSTR("Y-fsteps"),  &tmc2130_home_fsteps[Y_AXIS],  0, 128);
+	END_MENU();
+}
+
+static void lcd_homing_accuracy_menu()
+{
+	START_MENU();
+	MENU_ITEM(back, PSTR("Experimental"), lcd_experimantal_menu);
+	MENU_ITEM(function, tmc2130_home_enabled?PSTR("Accur. homing  On"):PSTR("Accur. homing Off"), lcd_accurate_home_set);
+    MENU_ITEM(gcode, PSTR("Calibrate X"), PSTR("G28XC"));
+    MENU_ITEM(gcode, PSTR("Calibrate Y"), PSTR("G28YC"));
+	MENU_ITEM(submenu, PSTR("Advanced"), lcd_homing_accuracy_menu_advanced);
+	END_MENU();
+}
+
+static void lcd_ustep_resolution_menu_save()
+{
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_X_MRES, tmc2130_mres[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Y_MRES, tmc2130_mres[Y_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_Z_MRES, tmc2130_mres[Z_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_E_MRES, tmc2130_mres[E_AXIS]);
+}
+
+static void lcd_ustep_resolution_menu_back()
+{
+	float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
+	bool changed = false;
+	if (tmc2130_mres[X_AXIS] != eeprom_read_byte((uint8_t*)EEPROM_TMC2130_X_MRES))
+	{
+		axis_steps_per_unit[X_AXIS] = tmp1[X_AXIS] * tmc2130_mres2usteps(tmc2130_mres[X_AXIS]) / TMC2130_USTEPS_XY;
+		changed = true;
+	}
+	if (tmc2130_mres[Y_AXIS] != eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Y_MRES))
+	{
+		axis_steps_per_unit[Y_AXIS] = tmp1[Y_AXIS] * tmc2130_mres2usteps(tmc2130_mres[Y_AXIS]) / TMC2130_USTEPS_XY;
+		changed = true;
+	}
+	if (tmc2130_mres[Z_AXIS] != eeprom_read_byte((uint8_t*)EEPROM_TMC2130_Z_MRES))
+	{
+		axis_steps_per_unit[Z_AXIS] = tmp1[Z_AXIS] * tmc2130_mres2usteps(tmc2130_mres[Z_AXIS]) / TMC2130_USTEPS_Z;
+		changed = true;
+	}
+	if (tmc2130_mres[E_AXIS] != eeprom_read_byte((uint8_t*)EEPROM_TMC2130_E_MRES))
+	{
+		axis_steps_per_unit[E_AXIS] = tmp1[E_AXIS] * tmc2130_mres2usteps(tmc2130_mres[E_AXIS]) / TMC2130_USTEPS_E;
+		changed = true;
+	}
+    if (changed)
+	{
+		lcd_ustep_resolution_menu_save();
+		Config_StoreSettings(EEPROM_OFFSET);
+		tmc2130_init();
+	}
+	currentMenu = lcd_experimantal_menu;
+	lcd_experimantal_menu();
+}
+
+static void lcd_ustep_resolution_reset_def_xyze()
+{
+	tmc2130_mres[X_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Y_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_XY);
+	tmc2130_mres[Z_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_Z);
+	tmc2130_mres[E_AXIS] = tmc2130_usteps2mres(TMC2130_USTEPS_E);
+	float tmp1[]=DEFAULT_AXIS_STEPS_PER_UNIT;
+	axis_steps_per_unit[X_AXIS] = tmp1[X_AXIS];
+	axis_steps_per_unit[Y_AXIS] = tmp1[Y_AXIS];
+	axis_steps_per_unit[Z_AXIS] = tmp1[Z_AXIS];
+	axis_steps_per_unit[E_AXIS] = tmp1[E_AXIS];
+}
+
+static void lcd_ustep_resolution_menu()
+{
+	lcd_timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
+	START_MENU();
+	MENU_ITEM(back, PSTR("Experimental"), lcd_ustep_resolution_menu_back);
+	MENU_ITEM(function, PSTR("Reset defaults"),  lcd_ustep_resolution_reset_def_xyze);
+	MENU_ITEM_EDIT(mres, PSTR("X-resolution"),  &tmc2130_mres[X_AXIS],  4, 4);
+	MENU_ITEM_EDIT(mres, PSTR("Y-resolution"),  &tmc2130_mres[Y_AXIS],  4, 4);
+	MENU_ITEM_EDIT(mres, PSTR("Z-resolution"),  &tmc2130_mres[Z_AXIS],  4, 4);
+	MENU_ITEM_EDIT(mres, PSTR("E-resolution"),  &tmc2130_mres[E_AXIS],  2, 5);
+	END_MENU();
+}
+
+static void lcd_ustep_linearity_menu_save()
+{
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_WAVE_X_FAC, tmc2130_wave_fac[X_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_WAVE_Y_FAC, tmc2130_wave_fac[Y_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_WAVE_Z_FAC, tmc2130_wave_fac[Z_AXIS]);
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_WAVE_E_FAC, tmc2130_wave_fac[E_AXIS]);
+}
+
+static void lcd_ustep_linearity_menu_back()
+{
+	bool changed = false;
+	if (tmc2130_wave_fac[X_AXIS] < TMC2130_WAVE_FAC200_MIN) tmc2130_wave_fac[X_AXIS] = 0;
+	if (tmc2130_wave_fac[Y_AXIS] < TMC2130_WAVE_FAC200_MIN) tmc2130_wave_fac[Y_AXIS] = 0;
+	if (tmc2130_wave_fac[Z_AXIS] < TMC2130_WAVE_FAC200_MIN) tmc2130_wave_fac[Z_AXIS] = 0;
+	if (tmc2130_wave_fac[E_AXIS] < TMC2130_WAVE_FAC200_MIN) tmc2130_wave_fac[E_AXIS] = 0;
+	changed |= (eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_X_FAC) != tmc2130_wave_fac[X_AXIS]);
+	changed |= (eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Y_FAC) != tmc2130_wave_fac[Y_AXIS]);
+	changed |= (eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_Z_FAC) != tmc2130_wave_fac[Z_AXIS]);
+	changed |= (eeprom_read_byte((uint8_t*)EEPROM_TMC2130_WAVE_E_FAC) != tmc2130_wave_fac[E_AXIS]);
+	lcd_ustep_linearity_menu_save();
+	if (changed) tmc2130_init();
+	currentMenu = lcd_experimantal_menu;
+	lcd_experimantal_menu();
+}
+
+static void lcd_ustep_linearity_menu_recomended()
+{
+	tmc2130_wave_fac[X_AXIS] = 220;
+	tmc2130_wave_fac[Y_AXIS] = 220;
+	tmc2130_wave_fac[Z_AXIS] = 220;
+	tmc2130_wave_fac[E_AXIS] = 220;
+}
+
+static void lcd_ustep_linearity_menu_reset()
+{
+	tmc2130_wave_fac[X_AXIS] = 0;
+	tmc2130_wave_fac[Y_AXIS] = 0;
+	tmc2130_wave_fac[Z_AXIS] = 0;
+	tmc2130_wave_fac[E_AXIS] = 0;
+}
+
+static void lcd_ustep_linearity_menu()
+{
+	lcd_timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
+	START_MENU();
+	MENU_ITEM(back, PSTR("Experimental"), lcd_ustep_linearity_menu_back);
+	MENU_ITEM(function, PSTR("Reset correction"), lcd_ustep_linearity_menu_reset);
+	MENU_ITEM(function, PSTR("Recomended config"), lcd_ustep_linearity_menu_recomended);
+	MENU_ITEM_EDIT(wfac, PSTR("X-correction"),  &tmc2130_wave_fac[X_AXIS],  TMC2130_WAVE_FAC200_MIN-TMC2130_WAVE_FAC200_STP, TMC2130_WAVE_FAC200_MAX);
+	MENU_ITEM_EDIT(wfac, PSTR("Y-correction"),  &tmc2130_wave_fac[Y_AXIS],  TMC2130_WAVE_FAC200_MIN-TMC2130_WAVE_FAC200_STP, TMC2130_WAVE_FAC200_MAX);
+	MENU_ITEM_EDIT(wfac, PSTR("Z-correction"),  &tmc2130_wave_fac[Z_AXIS],  TMC2130_WAVE_FAC200_MIN-TMC2130_WAVE_FAC200_STP, TMC2130_WAVE_FAC200_MAX);
+	MENU_ITEM_EDIT(wfac, PSTR("E-correction"),  &tmc2130_wave_fac[E_AXIS],  TMC2130_WAVE_FAC200_MIN-TMC2130_WAVE_FAC200_STP, TMC2130_WAVE_FAC200_MAX);
+	END_MENU();
+}
+
+static void lcd_experimantal_menu_save_all()
+{
+	eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_ENABLED, tmc2130_home_enabled);
+	lcd_ustep_resolution_menu_save();
+	lcd_ustep_linearity_menu_save();
+	Config_StoreSettings(EEPROM_OFFSET);
+}
+
+static void lcd_experimantal_menu_disable_all()
+{
+	tmc2130_home_enabled = 0;
+	lcd_ustep_resolution_reset_def_xyze();
+	lcd_ustep_linearity_menu_reset();
+	lcd_experimantal_menu_save_all();
+	tmc2130_init();
+}
+
+static void lcd_experimantal_menu()
+{
+	START_MENU();
+	MENU_ITEM(back, MSG_MAIN, lcd_main_menu);
+	MENU_ITEM(function, PSTR("All Xfeatures off"), lcd_experimantal_menu_disable_all);
+	MENU_ITEM(submenu, PSTR("Homing accuracy"), lcd_homing_accuracy_menu);
+	MENU_ITEM(submenu, PSTR("uStep resolution"), lcd_ustep_resolution_menu);
+	MENU_ITEM(submenu, PSTR("uStep linearity"), lcd_ustep_linearity_menu);
+	END_MENU();
+}
+
+
 static void lcd_calibration_menu()
 {
   START_MENU();
@@ -5155,6 +5371,7 @@ static void lcd_main_menu()
 	#endif
 	MENU_ITEM(submenu, MSG_SETTINGS, lcd_settings_menu);
     if(!isPrintPaused) MENU_ITEM(submenu, MSG_MENU_CALIBRATION, lcd_calibration_menu);
+	MENU_ITEM(submenu, PSTR("Experimantal"), lcd_experimantal_menu);
   }
 
   if (!is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
@@ -5585,6 +5802,30 @@ void lcd_sdcard_menu()
   }
   */
 
+// Convert tmc2130 mres to string 
+char *mres_to_str3(const uint8_t &x)
+{
+	return itostr3(256 >> x);
+}
+
+extern char conv[8];
+
+// Convert tmc2130 wfac to string 
+char *wfac_to_str5(const uint8_t &x)
+{
+	if (x>=TMC2130_WAVE_FAC200_MIN) return ftostr43(((float)(x & 0xff))/200);
+	conv[0] = ' ';
+	conv[1] = ' ';
+	conv[2] = 'O';
+	conv[3] = 'f';
+	conv[4] = 'f';
+	conv[5] = 0;
+	return conv;
+}
+
+menu_edit_type(uint8_t, wfac, wfac_to_str5, 1)
+menu_edit_type(uint8_t, mres, mres_to_str3, 1)
+menu_edit_type(uint8_t, byte3, itostr3, 1)
 menu_edit_type(int, int3, itostr3, 1)
 menu_edit_type(float, float3, ftostr3, 1)
 menu_edit_type(float, float32, ftostr32, 100)

Firmware/ultralcd_implementation_hitachi_HD44780.h

@@ -1144,6 +1144,17 @@ static void lcd_implementation_drawmenu_setting_edit_generic_P(uint8_t row, cons
         lcd.print(' ');
     lcd_printPGM(data);
 }
+
+
+extern char *wfac_to_str5(const uint8_t &x);
+extern char *mres_to_str3(const uint8_t &x);
+
+#define lcd_implementation_drawmenu_setting_edit_wfac_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', wfac_to_str5(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_wfac(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', wfac_to_str5(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_mres_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', mres_to_str3(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_mres(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', mres_to_str3(*(data)))
+#define lcd_implementation_drawmenu_setting_edit_byte3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', itostr3((uint8_t)*(data)))
+#define lcd_implementation_drawmenu_setting_edit_byte3(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', itostr3((uint8_t)*(data)))
 #define lcd_implementation_drawmenu_setting_edit_int3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_int3(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, ' ', itostr3(*(data)))
 #define lcd_implementation_drawmenu_setting_edit_float3_selected(row, pstr, pstr2, data, minValue, maxValue) lcd_implementation_drawmenu_setting_edit_generic(row, pstr, '>', ftostr3(*(data)))