diff --git a/conf_general.h b/conf_general.h
index b0205c39..3c50e238 100755
--- a/conf_general.h
+++ b/conf_general.h
@@ -120,6 +120,12 @@
 
 // Current ADC to amperes factor
 #define FAC_CURRENT					((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN))
+#define FAC_CURRENT1				(FAC_CURRENT * CURRENT_CAL1)
+#define FAC_CURRENT2				(FAC_CURRENT * CURRENT_CAL2)
+#define FAC_CURRENT3				(FAC_CURRENT * CURRENT_CAL3)
+#define FAC_CURRENT1_M2				(FAC_CURRENT * CURRENT_CAL1_M2)
+#define FAC_CURRENT2_M2				(FAC_CURRENT * CURRENT_CAL2_M2)
+#define FAC_CURRENT3_M2				(FAC_CURRENT * CURRENT_CAL3_M2)
 
 #define VOLTAGE_TO_ADC_FACTOR	( VIN_R2 / (VIN_R2 + VIN_R1) ) * ( 4096.0 / V_REG )
 
diff --git a/hwconf/hw.h b/hwconf/hw.h
index e05936b4..ecb0f990 100644
--- a/hwconf/hw.h
+++ b/hwconf/hw.h
@@ -331,56 +331,105 @@
 
 // Current ADC macros. Override them for custom current measurement functions.
 #ifndef GET_CURRENT1
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT1()		(4095.0 - (float)ADC_Value[ADC_IND_CURR1])
-#else
-#define GET_CURRENT1()		((float)ADC_Value[ADC_IND_CURR1])
-#endif
+	#ifdef INVERTED_SHUNT_POLARITY
+		#define GET_CURRENT1()		(4095.0 - (float)ADC_Value[ADC_IND_CURR1])
+	#else
+		#define GET_CURRENT1()		((float)ADC_Value[ADC_IND_CURR1])
+	#endif
 #endif
+
 #ifndef GET_CURRENT2
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT2()		(4095.0 - (float)ADC_Value[ADC_IND_CURR2])
+	#ifdef INVERTED_SHUNT_POLARITY
+		#define GET_CURRENT2()		(4095.0 - (float)ADC_Value[ADC_IND_CURR2])
+	#else
+		#define GET_CURRENT2()		((float)ADC_Value[ADC_IND_CURR2])
+	#endif
+#endif
+
+#ifdef HW_HAS_3_SHUNTS
+	#ifndef GET_CURRENT3	
+		#ifdef ADC_IND_CURR3
+			#ifdef INVERTED_SHUNT_POLARITY
+				#define GET_CURRENT3()		(4095.0 - (float)ADC_Value[ADC_IND_CURR3])
+			#else
+				#define GET_CURRENT3()		((float)ADC_Value[ADC_IND_CURR3])
+			#endif
+		#else
+			#define ADC_IND_CURR3		0
+			#define GET_CURRENT3()		0
+		#endif
+	#endif
 #else
-#define GET_CURRENT2()		((float)ADC_Value[ADC_IND_CURR2])
-#endif
-#endif
-#ifndef GET_CURRENT3
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT3()		(4095.0 - (float)ADC_Value[ADC_IND_CURR3])
-#else
-#ifdef ADC_IND_CURR3
-#define GET_CURRENT3()		((float)ADC_Value[ADC_IND_CURR3])
-#else
-#define ADC_IND_CURR3		0
-#define GET_CURRENT3()		0
-#endif
-#endif
+	#ifndef ADC_IND_CURR3	
+		#define ADC_IND_CURR3		0
+	#endif
+	#define GET_CURRENT3()		0
 #endif
 
 #ifndef GET_CURRENT1_M2
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT1_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR4])
-#else
-#define GET_CURRENT1_M2()	((float)ADC_Value[ADC_IND_CURR4])
-#endif
+	#ifdef ADC_IND_CURR4
+		#ifdef INVERTED_SHUNT_POLARITY
+			#define GET_CURRENT1_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR4])
+		#else
+			#define GET_CURRENT1_M2()	((float)ADC_Value[ADC_IND_CURR4])
+		#endif
+	#else
+		#define GET_CURRENT1_M2()	0
+		#define ADC_IND_CURR4		0
+	#endif
 #endif
+
 #ifndef GET_CURRENT2_M2
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT2_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR5])
+	#ifdef ADC_IND_CURR5
+		#ifdef INVERTED_SHUNT_POLARITY
+			#define GET_CURRENT2_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR5])
+		#else
+			#define GET_CURRENT2_M2()	((float)ADC_Value[ADC_IND_CURR5])
+		#endif
+	#else
+		#define GET_CURRENT2_M2()	0
+		#define ADC_IND_CURR5		0
+	#endif
+#endif
+
+#ifdef HW_HAS_3_SHUNTS
+	#ifndef GET_CURRENT3_M2
+		#ifdef ADC_IND_CURR6
+			#ifdef INVERTED_SHUNT_POLARITY
+				#define GET_CURRENT3_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR6])
+			#else
+				#define GET_CURRENT3_M2()	((float)ADC_Value[ADC_IND_CURR6])
+			#endif
+		#else
+			#define GET_CURRENT3_M2()	0
+			#define ADC_IND_CURR6		0
+		#endif		
+	#endif
 #else
-#define GET_CURRENT2_M2()	((float)ADC_Value[ADC_IND_CURR5])
+	#define GET_CURRENT3_M2()	0
+	#ifndef ADC_IND_CURR6	
+		#define ADC_IND_CURR6		0
+	#endif
 #endif
+
+
+#ifndef CURRENT_CAL1
+#define CURRENT_CAL1				1.0
 #endif
-#ifndef GET_CURRENT3_M2
-#ifdef INVERTED_SHUNT_POLARITY
-#define GET_CURRENT3_M2()	(4095.0 - (float)ADC_Value[ADC_IND_CURR6])
-#else
-#ifdef ADC_IND_CURR6
-#define GET_CURRENT3_M2()	((float)ADC_Value[ADC_IND_CURR6])
-#else
-#define GET_CURRENT3_M2()	0
+#ifndef CURRENT_CAL2
+#define CURRENT_CAL2				1.0
 #endif
+#ifndef CURRENT_CAL3
+#define CURRENT_CAL3				1.0
 #endif
+#ifndef CURRENT_CAL1_M2
+#define CURRENT_CAL1_M2				1.0
+#endif
+#ifndef CURRENT_CAL2_M2
+#define CURRENT_CAL2_M2				1.0
+#endif
+#ifndef CURRENT_CAL3_M2
+#define CURRENT_CAL3_M2				1.0
 #endif
 
 #ifndef HW_MAX_CURRENT_OFFSET
diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c
index 7ba86239..18955e41 100644
--- a/lispBM/lispif_vesc_extensions.c
+++ b/lispBM/lispif_vesc_extensions.c
@@ -2459,17 +2459,22 @@ static lbm_value ext_raw_adc_current(lbm_value *args, lbm_uint argn) {
 	mcpwm_foc_get_current_offsets(&ofs1, &ofs2, &ofs3, motor == 2);
 	float ph1, ph2, ph3;
 	mcpwm_foc_get_currents_adc(&ph1, &ph2, &ph3, motor == 2);
-	float scale = FAC_CURRENT;
-
+	float scale1, scale2, scale3;
+	if(motor == 2) {
+		scale1 = FAC_CURRENT1_M2; scale2 = FAC_CURRENT2_M2; scale3 = FAC_CURRENT3_M2;
+	} else {
+		scale1 = FAC_CURRENT1; scale2 = FAC_CURRENT2; scale3 = FAC_CURRENT3;
+	}
+	
 	if (argn == 3 && lbm_dec_as_i32(args[2]) != 0) {
-		scale = 1.0;
+		scale1 = 1.0; scale2 = 1.0; scale3 = 1.0;
 		ofs1 = 0.0; ofs2 = 0.0; ofs3 = 0.0;
 	}
 
 	switch(phase) {
-	case 1: return lbm_enc_float((ph1 - ofs1) * scale);
-	case 2: return lbm_enc_float((ph2 - ofs2) * scale);
-	case 3: return lbm_enc_float((ph3 - ofs3) * scale);
+	case 1: return lbm_enc_float((ph1 - ofs1) * scale1);
+	case 2: return lbm_enc_float((ph2 - ofs2) * scale2);
+	case 3: return lbm_enc_float((ph3 - ofs3) * scale3);
 	default: return ENC_SYM_EERROR;
 	}
 }
diff --git a/motor/mc_interface.c b/motor/mc_interface.c
index 3277d522..635ecd60 100644
--- a/motor/mc_interface.c
+++ b/motor/mc_interface.c
@@ -50,6 +50,8 @@
 // Global variables
 volatile uint16_t ADC_Value[HW_ADC_CHANNELS + HW_ADC_CHANNELS_EXTRA];
 volatile float ADC_curr_norm_value[6];
+volatile float ADC_curr_raw[6];
+
 
 typedef struct {
 	mc_configuration m_conf;
@@ -2162,8 +2164,8 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
 			}
 
 			if (state == MC_STATE_DETECTING) {
-				m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
-				m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
+				m_curr0_samples[m_sample_now] = (int16_t)(mcpwm_detect_currents[mcpwm_get_comm_step() - 1] * (8.0 / FAC_CURRENT));
+				m_curr1_samples[m_sample_now] = (int16_t)(mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1] * (8.0 / FAC_CURRENT));
 				m_curr2_samples[m_sample_now] = 0;
 
 				m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
@@ -2171,17 +2173,29 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
 				m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
 			} else {
 				if (is_second_motor) {
-					m_curr0_samples[m_sample_now] = ADC_curr_norm_value[3];
-					m_curr1_samples[m_sample_now] = ADC_curr_norm_value[4];
-					m_curr2_samples[m_sample_now] = ADC_curr_norm_value[5];
+					if (m_sample_raw) {
+						m_curr0_samples[m_sample_now] = ADC_curr_raw[3];
+						m_curr1_samples[m_sample_now] = ADC_curr_raw[4];
+						m_curr2_samples[m_sample_now] = ADC_curr_raw[5];
+					} else {
+						m_curr0_samples[m_sample_now] = ADC_curr_norm_value[3] * (8.0 / FAC_CURRENT);
+						m_curr1_samples[m_sample_now] = ADC_curr_norm_value[4] * (8.0 / FAC_CURRENT);
+						m_curr2_samples[m_sample_now] = ADC_curr_norm_value[5] * (8.0 / FAC_CURRENT);	
+					}
 
 					m_ph1_samples[m_sample_now] = ADC_V_L4 - zero;
 					m_ph2_samples[m_sample_now] = ADC_V_L5 - zero;
 					m_ph3_samples[m_sample_now] = ADC_V_L6 - zero;
 				} else {
-					m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
-					m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
-					m_curr2_samples[m_sample_now] = ADC_curr_norm_value[2];
+					if (m_sample_raw) {
+						m_curr0_samples[m_sample_now] = ADC_curr_raw[0];
+						m_curr1_samples[m_sample_now] = ADC_curr_raw[1];
+						m_curr2_samples[m_sample_now] = ADC_curr_raw[2];
+					} else {
+						m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0] * (8.0 / FAC_CURRENT);
+						m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1] * (8.0 / FAC_CURRENT);
+						m_curr2_samples[m_sample_now] = ADC_curr_norm_value[2] * (8.0 / FAC_CURRENT);
+					}					
 
 					m_ph1_samples[m_sample_now] = ADC_V_L1 - zero;
 					m_ph2_samples[m_sample_now] = ADC_V_L2 - zero;
@@ -2881,9 +2895,9 @@ static THD_FUNCTION(sample_send_thread, arg) {
 				buffer_append_float32_auto(buffer, (float)m_vzero_samples[ind_samp], &index);
 				buffer_append_float32_auto(buffer, (float)m_curr_fir_samples[ind_samp], &index);
 			} else {
-				buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp] * FAC_CURRENT, &index);
-				buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp] * FAC_CURRENT, &index);
-				buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp] * FAC_CURRENT, &index);
+				buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp] / (8.0 / FAC_CURRENT), &index);
+				buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp] / (8.0 / FAC_CURRENT), &index);
+				buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp] / (8.0 / FAC_CURRENT), &index);
 				buffer_append_float32_auto(buffer, ((float)m_ph1_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
 				buffer_append_float32_auto(buffer, ((float)m_ph2_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
 				buffer_append_float32_auto(buffer, ((float)m_ph3_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
diff --git a/motor/mc_interface.h b/motor/mc_interface.h
index 4ff70379..319e7745 100644
--- a/motor/mc_interface.h
+++ b/motor/mc_interface.h
@@ -140,6 +140,8 @@ void mc_interface_adc_inj_int_handler(void);
 // External variables
 extern volatile uint16_t ADC_Value[];
 extern volatile float ADC_curr_norm_value[];
+extern volatile float ADC_curr_raw[];
+
 
 // Common fixed parameters
 #ifndef HW_DEAD_TIME_NSEC
diff --git a/motor/mcpwm.c b/motor/mcpwm.c
index 70a2d72f..74c5462e 100644
--- a/motor/mcpwm.c
+++ b/motor/mcpwm.c
@@ -1457,14 +1457,14 @@ static THD_FUNCTION(timer_thread, arg) {
 void mcpwm_adc_inj_int_handler(void) {
 	uint32_t t_start = timer_time_now();
 
-	int curr0 = HW_GET_INJ_CURR1();
-	int curr1 = HW_GET_INJ_CURR2();
+	float curr0 = HW_GET_INJ_CURR1();
+	float curr1 = HW_GET_INJ_CURR2();
 
-	int curr0_2 = HW_GET_INJ_CURR1_S2();
-	int curr1_2 = HW_GET_INJ_CURR1_S2();
+	float curr0_2 = HW_GET_INJ_CURR1_S2();
+	float curr1_2 = HW_GET_INJ_CURR1_S2();
 
 #ifdef HW_HAS_3_SHUNTS
-	int curr2 = HW_GET_INJ_CURR3();
+	float curr2 = HW_GET_INJ_CURR3();
 #endif
 
 #ifdef INVERTED_SHUNT_POLARITY
@@ -1549,25 +1549,37 @@ void mcpwm_adc_inj_int_handler(void) {
 	}
 #endif
 
+	// Store raw ADC readings for raw sampling mode.
+	ADC_curr_raw[0] = curr0;
+	ADC_curr_raw[1] = curr1;
+#ifdef HW_HAS_3_SHUNTS
+	ADC_curr_raw[2] = curr2;
+#endif
+	
+	// Scale to AMPs using calibrated scaling factors	
+	curr0 *= FAC_CURRENT1;
+	curr1 *= FAC_CURRENT2;	
+#ifdef HW_HAS_3_SHUNTS
+	curr2 *= FAC_CURRENT3;
+#else
+	int curr2 = -(curr0 + curr1);
+#endif
+
+	// Store the currents for sampling
 	ADC_curr_norm_value[0] = curr0;
 	ADC_curr_norm_value[1] = curr1;
-
-#ifdef HW_HAS_3_SHUNTS
-	ADC_curr_norm_value[2] = curr2;
-#else
-	ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
-#endif
+	ADC_curr_norm_value[2] = curr2;	
 
 	float curr_tot_sample = 0;
 	if (conf->motor_type == MOTOR_TYPE_DC) {
 		if (direction) {
 #ifdef HW_HAS_3_SHUNTS
-			curr_tot_sample = -(GET_CURRENT3() - curr2_offset);
+			curr_tot_sample = -(GET_CURRENT3() - curr2_offset) * FAC_CURRENT3;
 #else
-			curr_tot_sample = -(GET_CURRENT2() - curr1_offset);
+			curr_tot_sample = -(GET_CURRENT2() - curr1_offset) * FAC_CURRENT2;
 #endif
 		} else {
-			curr_tot_sample = -(GET_CURRENT1() - curr0_offset);
+			curr_tot_sample = -(GET_CURRENT1() - curr0_offset) * FAC_CURRENT1;
 		}
 	} else {
 		static int detect_now = 0;
@@ -1593,52 +1605,52 @@ void mcpwm_adc_inj_int_handler(void) {
 		 */
 
 		if (state == MC_STATE_FULL_BRAKE) {
-			float c0 = (float)ADC_curr_norm_value[0];
-			float c1 = (float)ADC_curr_norm_value[1];
-			float c2 = (float)ADC_curr_norm_value[2];
+			float c0 = (float)curr0;
+			float c1 = (float)curr1;
+			float c2 = (float)curr2;
 			curr_tot_sample = sqrtf((c0*c0 + c1*c1 + c2*c2) / 1.5);
 		} else {
 #ifdef HW_HAS_3_SHUNTS
 			if (direction) {
 				switch (comm_step) {
-				case 1: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
-				case 2: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
-				case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
-				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
+				case 1: curr_tot_sample = -(float)curr2; break;
+				case 2: curr_tot_sample = -(float)curr2; break;
+				case 3: curr_tot_sample = -(float)curr1; break;
+				case 4: curr_tot_sample = -(float)curr1; break;
+				case 5: curr_tot_sample = -(float)curr0; break;
+				case 6: curr_tot_sample = -(float)curr0; break;
 				default: break;
 				}
 			} else {
 				switch (comm_step) {
-				case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 3: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
-				case 4: curr_tot_sample = -(float)ADC_curr_norm_value[2]; break;
-				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
-				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
+				case 1: curr_tot_sample = -(float)curr1; break;
+				case 2: curr_tot_sample = -(float)curr1; break;
+				case 3: curr_tot_sample = -(float)curr2; break;
+				case 4: curr_tot_sample = -(float)curr2; break;
+				case 5: curr_tot_sample = -(float)curr0; break;
+				case 6: curr_tot_sample = -(float)curr0; break;
 				default: break;
 				}
 			}
 #else
 			if (direction) {
 				switch (comm_step) {
-				case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 3: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
-				case 4: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
-				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
-				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
+				case 1: curr_tot_sample = -(float)curr1; break;
+				case 2: curr_tot_sample = -(float)curr1; break;
+				case 3: curr_tot_sample = (float)curr0; break;
+				case 4: curr_tot_sample = (float)curr1; break;
+				case 5: curr_tot_sample = -(float)curr0; break;
+				case 6: curr_tot_sample = -(float)curr0; break;
 				default: break;
 				}
 			} else {
 				switch (comm_step) {
-				case 1: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
-				case 2: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
-				case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
-				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
-				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
+				case 1: curr_tot_sample = (float)curr1; break;
+				case 2: curr_tot_sample = (float)curr0; break;
+				case 3: curr_tot_sample = -(float)curr1; break;
+				case 4: curr_tot_sample = -(float)curr1; break;
+				case 5: curr_tot_sample = -(float)curr0; break;
+				case 6: curr_tot_sample = -(float)curr0; break;
 				default: break;
 				}
 			}
@@ -1655,8 +1667,8 @@ void mcpwm_adc_inj_int_handler(void) {
 		}
 
 		if (detect_now == 4) {
-			const float a = fabsf(ADC_curr_norm_value[0]);
-			const float b = fabsf(ADC_curr_norm_value[1]);
+			const float a = fabsf(curr0);
+			const float b = fabsf(curr1);
 
 			if (a > b) {
 				mcpwm_detect_currents[detect_step] = a;
@@ -1722,7 +1734,7 @@ void mcpwm_adc_inj_int_handler(void) {
 		}
 	}
 
-	last_current_sample = curr_tot_sample * FAC_CURRENT;
+	last_current_sample = curr_tot_sample;
 
 	// Filter out outliers
 	if (fabsf(last_current_sample) > (conf->l_abs_current_max * 1.2)) {
diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c
index cf8b75fd..2f22dbc8 100644
--- a/motor/mcpwm_foc.c
+++ b/motor/mcpwm_foc.c
@@ -2797,9 +2797,9 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 
 	bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
 	int norm_curr_ofs = 0;
-
+	bool is_second_motor = 0;
 #ifdef HW_HAS_DUAL_MOTORS
-	bool is_second_motor = is_v7;
+	is_second_motor = is_v7;
 	norm_curr_ofs = is_second_motor ? 3 : 0;
 	motor_all_state_t *motor_now = is_second_motor ? (motor_all_state_t*)&m_motor_2 : (motor_all_state_t*)&m_motor_1;
 	motor_all_state_t *motor_other = is_second_motor ? (motor_all_state_t*)&m_motor_1 : (motor_all_state_t*)&m_motor_2;
@@ -2830,17 +2830,17 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		float curr1;
 
 		if (is_second_motor) {
-			curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
-			curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
+			curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT1;
+			curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT2;
 			TIMER_UPDATE_DUTY_M1(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
 		} else {
-			curr0 = (GET_CURRENT1_M2() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
-			curr1 = (GET_CURRENT2_M2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
+			curr0 = (GET_CURRENT1_M2() - conf_other->foc_offsets_current[0]) * FAC_CURRENT1_M2;
+			curr1 = (GET_CURRENT2_M2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT2_M2;
 			TIMER_UPDATE_DUTY_M2(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
 		}
 #else
-		float curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT;
-		float curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT;
+		float curr0 = (GET_CURRENT1() - conf_other->foc_offsets_current[0]) * FAC_CURRENT1;
+		float curr1 = (GET_CURRENT2() - conf_other->foc_offsets_current[1]) * FAC_CURRENT2;
 
 		TIMER_UPDATE_DUTY_M1(motor_other->m_duty1_next, motor_other->m_duty2_next, motor_other->m_duty3_next);
 #ifdef HW_HAS_DUAL_PARALLEL
@@ -2923,58 +2923,62 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	palClearPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);
 #endif
 
-#ifdef HW_HAS_DUAL_MOTORS
 	float curr0 = 0;
 	float curr1 = 0;
+	float curr2 = 0;
 
+// Get ADC readings 0-4095
 	if (is_second_motor) {
 		curr0 = GET_CURRENT1_M2();
 		curr1 = GET_CURRENT2_M2();
+		curr2 = GET_CURRENT3_M2();
 	} else {
 		curr0 = GET_CURRENT1();
 		curr1 = GET_CURRENT2();
+		curr2 = GET_CURRENT3();
 	}
-#else
-	float curr0 = GET_CURRENT1();
-	float curr1 = GET_CURRENT2();
+
 #ifdef HW_HAS_DUAL_PARALLEL
+	// Add both currents together
 	curr0 += GET_CURRENT1_M2();
 	curr1 += GET_CURRENT2_M2();
-#endif
-#endif
-
-#ifdef HW_HAS_3_SHUNTS
-#ifdef HW_HAS_DUAL_MOTORS
-	float curr2 = is_second_motor ? GET_CURRENT3_M2() : GET_CURRENT3();
-#else
-	float curr2 = GET_CURRENT3();
-#ifdef HW_HAS_DUAL_PARALLEL
-	curr2 += GET_CURRENT3_M2();
-#endif
-#endif
+	curr2 += GET_CURRENT3_M2();	
 #endif
 
+// Store raw ADC readings
 	motor_now->m_currents_adc[0] = curr0;
 	motor_now->m_currents_adc[1] = curr1;
-#ifdef HW_HAS_3_SHUNTS
 	motor_now->m_currents_adc[2] = curr2;
-#else
-	motor_now->m_currents_adc[2] = 0.0;
-#endif
 
+// Shift to midpoint using offset (should be close to 2048)
 	curr0 -= conf_now->foc_offsets_current[0];
 	curr1 -= conf_now->foc_offsets_current[1];
-#ifdef HW_HAS_3_SHUNTS
 	curr2 -= conf_now->foc_offsets_current[2];
+	
+// Store midshifted raw ADC readings for raw sampling mode.
+	ADC_curr_raw[0 + norm_curr_ofs] = curr0;
+	ADC_curr_raw[1 + norm_curr_ofs] = curr1;
+	ADC_curr_raw[2 + norm_curr_ofs] = curr2;
+	
+#ifdef HW_HAS_3_SHUNTS	
+	// Calculate unbalance to detect bad sensor
 	motor_now->m_curr_unbalance = curr0 + curr1 + curr2;
-#endif
+#endif	
 
-	ADC_curr_norm_value[0 + norm_curr_ofs] = curr0;
-	ADC_curr_norm_value[1 + norm_curr_ofs] = curr1;
-#ifdef HW_HAS_3_SHUNTS
-	ADC_curr_norm_value[2 + norm_curr_ofs] = curr2;
-#else
-	ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
+// Scale to AMPs using calibrated scaling factors	
+	if (is_second_motor) {
+		curr0 *= FAC_CURRENT1_M2;
+		curr1 *= FAC_CURRENT2_M2;
+		curr2 *= FAC_CURRENT3_M2;
+	} else {
+		curr0 *= FAC_CURRENT1;
+		curr1 *= FAC_CURRENT2;
+		curr2 *= FAC_CURRENT3;
+	}
+	
+#ifndef HW_HAS_3_SHUNTS	
+	// Calculate third current assuming they are balanced
+	curr2 = -(curr0 + curr1);
 #endif
 
 	// Use the best current samples depending on the modulation state.
@@ -2982,16 +2986,16 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	if (conf_now->foc_current_sample_mode == FOC_CURRENT_SAMPLE_MODE_HIGH_CURRENT) {
 		// High current sampling mode. Choose the lower currents to derive the highest one
 		// in order to be able to measure higher currents.
-		const float i0_abs = fabsf(ADC_curr_norm_value[0 + norm_curr_ofs]);
-		const float i1_abs = fabsf(ADC_curr_norm_value[1 + norm_curr_ofs]);
-		const float i2_abs = fabsf(ADC_curr_norm_value[2 + norm_curr_ofs]);
+		const float i0_abs = fabsf(curr0);
+		const float i1_abs = fabsf(curr1);
+		const float i2_abs = fabsf(curr2);
 
 		if (i0_abs > i1_abs && i0_abs > i2_abs) {
-			ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+			curr0 = -(curr1 + curr2);
 		} else if (i1_abs > i0_abs && i1_abs > i2_abs) {
-			ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+			curr1 = -(curr0 + curr2);
 		} else if (i2_abs > i0_abs && i2_abs > i1_abs) {
-			ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+			curr2 = -(curr0 + curr1);
 		}
 	} else if (conf_now->foc_current_sample_mode == FOC_CURRENT_SAMPLE_MODE_LONGEST_ZERO) {
 #ifdef HW_HAS_PHASE_SHUNTS
@@ -2999,28 +3003,28 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 			if (tim->CCR1 > 500 && tim->CCR2 > 500) {
 				// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
 				// This is especially important when using HFI.
-				ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+				curr2 = -(curr0 + curr1);
 			} else {
 				if (tim->CCR1 < tim->CCR2 && tim->CCR1 < tim->CCR3) {
-					ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+					curr0 = -(curr1 + curr2);
 				} else if (tim->CCR2 < tim->CCR1 && tim->CCR2 < tim->CCR3) {
-					ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+					curr1 = -(curr0 + curr2);
 				} else if (tim->CCR3 < tim->CCR1 && tim->CCR3 < tim->CCR2) {
-					ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+					curr2 = -(curr0 + curr1);
 				}
 			}
 		} else {
 			if (tim->CCR1 < (tim->ARR - 500) && tim->CCR2 < (tim->ARR - 500)) {
 				// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
 				// This is especially important when using HFI.
-				ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+				curr2 = -(curr0 + curr1);
 			} else {
 				if (tim->CCR1 > tim->CCR2 && tim->CCR1 > tim->CCR3) {
-					ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+					curr0 = -(curr1 + curr2);
 				} else if (tim->CCR2 > tim->CCR1 && tim->CCR2 > tim->CCR3) {
-					ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+					curr1 = -(curr0 + curr2);
 				} else if (tim->CCR3 > tim->CCR1 && tim->CCR3 > tim->CCR2) {
-					ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+					curr2 = -(curr0 + curr1);
 				}
 			}
 		}
@@ -3028,23 +3032,28 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		if (tim->CCR1 < (tim->ARR - 500) && tim->CCR2 < (tim->ARR - 500)) {
 			// Use the same 2 shunts on low modulation, as that will avoid jumps in the current reading.
 			// This is especially important when using HFI.
-			ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+			curr2 = -(curr0 + curr1);
 		} else {
 			if (tim->CCR1 > tim->CCR2 && tim->CCR1 > tim->CCR3) {
-				ADC_curr_norm_value[0 + norm_curr_ofs] = -(ADC_curr_norm_value[1 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+				curr0 = -(curr1 + curr2);
 			} else if (tim->CCR2 > tim->CCR1 && tim->CCR2 > tim->CCR3) {
-				ADC_curr_norm_value[1 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[2 + norm_curr_ofs]);
+				curr1 = -(curr0 + curr2);
 			} else if (tim->CCR3 > tim->CCR1 && tim->CCR3 > tim->CCR2) {
-				ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0 + norm_curr_ofs] + ADC_curr_norm_value[1 + norm_curr_ofs]);
+				curr2 = -(curr0 + curr1);
 			}
 		}
 #endif
 	}
 #endif
-
-	float ia = ADC_curr_norm_value[0 + norm_curr_ofs] * FAC_CURRENT;
-	float ib = ADC_curr_norm_value[1 + norm_curr_ofs] * FAC_CURRENT;
-	float ic = ADC_curr_norm_value[2 + norm_curr_ofs] * FAC_CURRENT;
+	
+	// Store the currents for sampling
+	ADC_curr_norm_value[0 + norm_curr_ofs] = curr0;
+	ADC_curr_norm_value[1 + norm_curr_ofs] = curr1;
+	ADC_curr_norm_value[2 + norm_curr_ofs] = curr2;
+	
+	float ia = curr0;
+	float ib = curr1;
+	float ic = curr2;
 
 	// This has to be done for the skip function to have any chance at working with the
 	// observer and control loops.