From 41b885c537c74d6e589c3dadedf4d7f2def808d3 Mon Sep 17 00:00:00 2001
From: Robert Pelnar <robert.pelnar@seznam.cz>
Date: Mon, 12 Nov 2018 20:39:25 +0100
Subject: [PATCH] M907 fix - value in mA (PFW)

---
 Firmware/Marlin_main.cpp |  8 ++++----
 Firmware/tmc2130.cpp     | 24 ++++++++++++++++++++++++
 Firmware/tmc2130.h       |  3 +++
 3 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index ffa12d38..abaca9f8 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -6632,10 +6632,10 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
         for (int i = 0; i < NUM_AXIS; i++)
 			if(code_seen(axis_codes[i]))
 			{
-				long cur = code_value_long();
-				if (cur > MOTOR_CURRENT_PWM_RANGE) cur = MOTOR_CURRENT_PWM_RANGE;
-				tmc2130_set_current_h(i, (uint8_t)cur);
-				tmc2130_set_current_r(i, (uint8_t)cur);
+				long cur_mA = code_value_long();
+				uint8_t val = tmc2130_cur2val(cur_mA);
+				tmc2130_set_current_h(i, val);
+				tmc2130_set_current_r(i, val);
 			}
 
 #else //TMC2130
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 92d24003..8020559c 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -998,4 +998,28 @@ bool tmc2130_home_calibrate(uint8_t axis)
 	return true;
 }
 
+uint8_t tmc2130_cur2val(float cur)
+{
+	if (cur < 0) cur = 0; //limit min
+	if (cur > 1029) cur = 1029; //limit max
+	//540mA is threshold for switch from high sense to low sense
+	//for higher currents is maximum current 1029mA
+	if (cur >= 540) return 63 * (float)cur / 1029;
+	//for lower currents must be the value divided by 1.125 (= 0.18*2/0.32)
+	return 63 * (float)cur / (1029 * 1.125);
+}
+
+float tmc2130_val2cur(uint8_t val)
+{
+	float rsense = 0.2; //0.2 ohm sense resistors
+	uint8_t vsense = (val & 0x20)?0:1; //vsense bit = val>31
+	float vfs = vsense?0.18:0.32; //vfs depends on vsense bit
+	uint8_t val2 = vsense?val:(val >> 1); //vals 32..63 shifted right (16..31)
+	// equation from datasheet (0.7071 ~= 1/sqrt(2))
+	float cur = ((float)(val2 + 1)/32) * (vfs/(rsense + 0.02)) * 0.7071;
+	return cur * 1000; //return current in mA
+}
+
+
+
 #endif //TMC2130
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index 66c2b92b..1c854f79 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -106,4 +106,7 @@ extern void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000);
 
 extern bool tmc2130_home_calibrate(uint8_t axis);
 
+extern uint8_t tmc2130_cur2val(float cur);
+extern float tmc2130_val2cur(uint8_t val);
+
 #endif //TMC2130_H