From af65f521dec573edb853f12bcd1da6dad58d318e Mon Sep 17 00:00:00 2001 From: Euan Date: Mon, 5 Feb 2024 23:47:45 +0000 Subject: [PATCH 01/10] Add support for calibrating current sensors Hw_conf can define CURRENT_CAL1,2,3 to set individual calibrations for each current sensor. By default no calibration is used. (x1.0) For example this is used on the A50S by looping a calibrated current using a power supply through each phase. Then the correction is saved in external eeprom to persist across firmware updates. Tested on FOC, DC and BLDC modes. --- conf_general.h | 3 ++ hwconf/hw.h | 10 ++++++ lispBM/lispif_vesc_extensions.c | 11 +++--- motor/mc_interface.c | 6 ++-- motor/mcpwm.c | 62 ++++++++++++++++----------------- motor/mcpwm_foc.c | 14 ++++---- 6 files changed, 60 insertions(+), 46 deletions(-) diff --git a/conf_general.h b/conf_general.h index b0205c39..e7b6e6fd 100755 --- a/conf_general.h +++ b/conf_general.h @@ -120,6 +120,9 @@ // Current ADC to amperes factor #define FAC_CURRENT ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN)) +#define FAC_CURRENT1 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL1 +#define FAC_CURRENT2 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL2 +#define FAC_CURRENT3 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL3 #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..8f1e3c12 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -383,6 +383,16 @@ #endif #endif +#ifndef CURRENT_CAL1 +#define CURRENT_CAL1 1.0 +#endif +#ifndef CURRENT_CAL2 +#define CURRENT_CAL2 1.0 +#endif +#ifndef CURRENT_CAL3 +#define CURRENT_CAL3 1.0 +#endif + #ifndef HW_MAX_CURRENT_OFFSET #define HW_MAX_CURRENT_OFFSET 620 #endif diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index 1c2d2bca..a3dafff4 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -2442,17 +2442,18 @@ 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; + 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..6e8d5c1f 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -2881,9 +2881,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] * FAC_CURRENT1, &index); + buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp] * FAC_CURRENT2, &index); + buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp] * FAC_CURRENT3, &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/mcpwm.c b/motor/mcpwm.c index 70a2d72f..c35e166c 100644 --- a/motor/mcpwm.c +++ b/motor/mcpwm.c @@ -1562,12 +1562,12 @@ void mcpwm_adc_inj_int_handler(void) { 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 +1593,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)ADC_curr_norm_value[0] * FAC_CURRENT1; + float c1 = (float)ADC_curr_norm_value[1] * FAC_CURRENT2; + float c2 = (float)ADC_curr_norm_value[2] * FAC_CURRENT3; 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)ADC_curr_norm_value[2] * FAC_CURRENT3; break; + case 2: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; + case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; 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)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 3: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; + case 4: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; + case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; 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)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 3: curr_tot_sample = (float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 4: curr_tot_sample = (float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; 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)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 2: curr_tot_sample = (float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; + case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; default: break; } } @@ -1722,7 +1722,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..7cb5e809 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -2830,8 +2830,8 @@ 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; @@ -2839,8 +2839,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { 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 @@ -3042,9 +3042,9 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { } #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; + float ia = ADC_curr_norm_value[0 + norm_curr_ofs] * FAC_CURRENT1; + float ib = ADC_curr_norm_value[1 + norm_curr_ofs] * FAC_CURRENT2; + float ic = ADC_curr_norm_value[2 + norm_curr_ofs] * FAC_CURRENT3; // This has to be done for the skip function to have any chance at working with the // observer and control loops. From 8580ec8c77f71051fa00ec5b20372b93865d84d8 Mon Sep 17 00:00:00 2001 From: Euan Date: Tue, 6 Feb 2024 00:14:54 +0000 Subject: [PATCH 02/10] Fix typo --- motor/mcpwm.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/motor/mcpwm.c b/motor/mcpwm.c index c35e166c..d30a89ff 100644 --- a/motor/mcpwm.c +++ b/motor/mcpwm.c @@ -1562,12 +1562,12 @@ void mcpwm_adc_inj_int_handler(void) { if (conf->motor_type == MOTOR_TYPE_DC) { if (direction) { #ifdef HW_HAS_3_SHUNTS - curr_tot_sample = -(GET_CURRENT3() - curr2_offset) * FAC_CURRENT3;; + curr_tot_sample = -(GET_CURRENT3() - curr2_offset) * FAC_CURRENT3; #else - curr_tot_sample = -(GET_CURRENT2() - curr1_offset) * FAC_CURRENT2;; + curr_tot_sample = -(GET_CURRENT2() - curr1_offset) * FAC_CURRENT2; #endif } else { - curr_tot_sample = -(GET_CURRENT1() - curr0_offset) * FAC_CURRENT1;; + curr_tot_sample = -(GET_CURRENT1() - curr0_offset) * FAC_CURRENT1; } } else { static int detect_now = 0; From c2878474a1f53f61c91d1defeb537c2c1067c0b2 Mon Sep 17 00:00:00 2001 From: Euan Date: Tue, 6 Feb 2024 21:49:38 +0000 Subject: [PATCH 03/10] Refactor mcpwm_foc and fix bug in hw.h Removed a bunch of ifdefs by making is_second_motor = 0 when not using dual motors. Fixed a bug in hw.h where it wrongly defined GET_CURRENT3 when inverted shunts was selected. GET_CURRENT3 will now always be 0 by default, removing the need for a few ifdefs in the foc code. --- hwconf/hw.h | 102 ++++++++++++++++++-------------- lispBM/lispif_vesc_extensions.c | 8 ++- motor/mcpwm_foc.c | 61 ++++++++++--------- 3 files changed, 97 insertions(+), 74 deletions(-) diff --git a/hwconf/hw.h b/hwconf/hw.h index 8f1e3c12..0024b5f6 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -331,58 +331,65 @@ // 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 -#endif -#ifndef GET_CURRENT2 -#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 -#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 + #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]) + #else + #define GET_CURRENT2() ((float)ADC_Value[ADC_IND_CURR2]) + #endif +#endif + +#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 + + #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 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 #endif + #ifndef GET_CURRENT2_M2 -#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 + #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 #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 -#endif -#endif + #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 + #endif + #endif #endif + #ifndef CURRENT_CAL1 #define CURRENT_CAL1 1.0 #endif @@ -392,6 +399,15 @@ #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 #define HW_MAX_CURRENT_OFFSET 620 diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index a3dafff4..ac8ee2b0 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -2443,8 +2443,12 @@ static lbm_value ext_raw_adc_current(lbm_value *args, lbm_uint argn) { float ph1, ph2, ph3; mcpwm_foc_get_currents_adc(&ph1, &ph2, &ph3, motor == 2); float scale1, scale2, scale3; - scale1 = FAC_CURRENT1; scale2 = FAC_CURRENT2; scale3 = FAC_CURRENT3; - + 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) { scale1 = 1.0; scale2 = 1.0; scale3 = 1.0; ofs1 = 0.0; ofs2 = 0.0; ofs3 = 0.0; diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index 7cb5e809..8e257cb7 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; @@ -2808,6 +2808,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { volatile TIM_TypeDef *tim = is_second_motor ? TIM8 : TIM1; #endif #else + motor_all_state_t *motor_other = (motor_all_state_t*)&m_motor_1; motor_all_state_t *motor_now = (motor_all_state_t*)&m_motor_1;; m_isr_motor = 1; @@ -2834,8 +2835,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { 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 @@ -2923,51 +2924,53 @@ 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 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]; + +#ifdef HW_HAS_3_SHUNTS + // Calculate unbalance to detect bad sensor motor_now->m_curr_unbalance = curr0 + curr1 + curr2; -#endif +#endif + +// 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; + } ADC_curr_norm_value[0 + norm_curr_ofs] = curr0; ADC_curr_norm_value[1 + norm_curr_ofs] = curr1; @@ -3042,9 +3045,9 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { } #endif - float ia = ADC_curr_norm_value[0 + norm_curr_ofs] * FAC_CURRENT1; - float ib = ADC_curr_norm_value[1 + norm_curr_ofs] * FAC_CURRENT2; - float ic = ADC_curr_norm_value[2 + norm_curr_ofs] * FAC_CURRENT3; + float ia = ADC_curr_norm_value[0 + norm_curr_ofs]; + float ib = ADC_curr_norm_value[1 + norm_curr_ofs]; + float ic = ADC_curr_norm_value[2 + norm_curr_ofs]; // This has to be done for the skip function to have any chance at working with the // observer and control loops. From 0cd896ab5b59a5e48819429e300bb5727302ec53 Mon Sep 17 00:00:00 2001 From: Euan Date: Tue, 6 Feb 2024 22:16:09 +0000 Subject: [PATCH 04/10] make GET_CURRENT3 = 0 when not on 3 shunt hw Some hw defines ADC_IND_CURR3 without defining HW_HAS_3_SHUNTS. So need to force GET_CURRENT3 to 0 in that case. --- hwconf/hw.h | 39 +++++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/hwconf/hw.h b/hwconf/hw.h index 0024b5f6..ed8181a7 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -346,20 +346,23 @@ #endif #endif -#ifndef GET_CURRENT3 - #ifdef ADC_IND_CURR3 - #ifdef INVERTED_SHUNT_POLARITY - #define GET_CURRENT3() (4095.0 - (float)ADC_Value[ADC_IND_CURR3]) +#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 GET_CURRENT3() ((float)ADC_Value[ADC_IND_CURR3]) + #define ADC_IND_CURR3 0 + #define GET_CURRENT3() 0 #endif - #else - #define ADC_IND_CURR3 0 - #define GET_CURRENT3() 0 #endif +#else + #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]) @@ -376,17 +379,21 @@ #endif #endif -#ifndef GET_CURRENT3_M2 - #ifdef ADC_IND_CURR6 - #ifdef INVERTED_SHUNT_POLARITY - #define GET_CURRENT3_M2() (4095.0 - (float)ADC_Value[ADC_IND_CURR6]) +#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() ((float)ADC_Value[ADC_IND_CURR6]) + #define GET_CURRENT3_M2() 0 #endif - #else - #define GET_CURRENT3_M2() 0 #endif #endif +#else + #define GET_CURRENT3_M2() 0 #endif From 5af6dbd03c277589393aaa99a02445d83d83fdc7 Mon Sep 17 00:00:00 2001 From: Euan Date: Tue, 6 Feb 2024 22:23:53 +0000 Subject: [PATCH 05/10] Add missing M2 defines and reduce code duplication --- conf_general.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/conf_general.h b/conf_general.h index e7b6e6fd..3c50e238 100755 --- a/conf_general.h +++ b/conf_general.h @@ -120,9 +120,12 @@ // Current ADC to amperes factor #define FAC_CURRENT ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN)) -#define FAC_CURRENT1 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL1 -#define FAC_CURRENT2 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL2 -#define FAC_CURRENT3 ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN )) * CURRENT_CAL3 +#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 ) From 5b3906f9d8b90be1ebee166a5fbb916660996ea2 Mon Sep 17 00:00:00 2001 From: Euan Date: Tue, 6 Feb 2024 22:51:19 +0000 Subject: [PATCH 06/10] Simplify mcpwm calibration. Make current samples correctly scaled. --- motor/mc_interface.c | 12 ++++----- motor/mcpwm.c | 60 ++++++++++++++++++++++---------------------- motor/mcpwm_foc.c | 1 - 3 files changed, 36 insertions(+), 37 deletions(-) diff --git a/motor/mc_interface.c b/motor/mc_interface.c index 6e8d5c1f..a9bfdfbb 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -2872,18 +2872,18 @@ static THD_FUNCTION(sample_send_thread, arg) { buffer[index++] = COMM_SAMPLE_PRINT; if (m_sample_raw) { - buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp], &index); - buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp], &index); - buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp], &index); + buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp] / FAC_CURRENT, &index); // TODO: Make this accurate for calibrated measurements. + 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_ph1_samples[ind_samp], &index); buffer_append_float32_auto(buffer, (float)m_ph2_samples[ind_samp], &index); buffer_append_float32_auto(buffer, (float)m_ph3_samples[ind_samp], &index); 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_CURRENT1, &index); - buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp] * FAC_CURRENT2, &index); - buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp] * FAC_CURRENT3, &index); + buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp], &index); + buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp], &index); + buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp], &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/mcpwm.c b/motor/mcpwm.c index d30a89ff..a171e937 100644 --- a/motor/mcpwm.c +++ b/motor/mcpwm.c @@ -1549,11 +1549,11 @@ void mcpwm_adc_inj_int_handler(void) { } #endif - ADC_curr_norm_value[0] = curr0; - ADC_curr_norm_value[1] = curr1; + ADC_curr_norm_value[0] = curr0 * FAC_CURRENT1; + ADC_curr_norm_value[1] = curr1 * FAC_CURRENT2; #ifdef HW_HAS_3_SHUNTS - ADC_curr_norm_value[2] = curr2; + ADC_curr_norm_value[2] = curr2 * FAC_CURRENT3; #else ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); #endif @@ -1593,52 +1593,52 @@ void mcpwm_adc_inj_int_handler(void) { */ if (state == MC_STATE_FULL_BRAKE) { - float c0 = (float)ADC_curr_norm_value[0] * FAC_CURRENT1; - float c1 = (float)ADC_curr_norm_value[1] * FAC_CURRENT2; - float c2 = (float)ADC_curr_norm_value[2] * FAC_CURRENT3; + float c0 = (float)ADC_curr_norm_value[0]; + float c1 = (float)ADC_curr_norm_value[1]; + float c2 = (float)ADC_curr_norm_value[2]; 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] * FAC_CURRENT3; break; - case 2: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; - case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + 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; default: break; } } else { switch (comm_step) { - case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 3: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; - case 4: curr_tot_sample = -(float)ADC_curr_norm_value[2] * FAC_CURRENT3; break; - case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + 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; default: break; } } #else if (direction) { switch (comm_step) { - case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 3: curr_tot_sample = (float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 4: curr_tot_sample = (float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + 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; default: break; } } else { switch (comm_step) { - case 1: curr_tot_sample = (float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 2: curr_tot_sample = (float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1] * FAC_CURRENT2; break; - case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; - case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0] * FAC_CURRENT1; break; + 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; default: break; } } diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index 8e257cb7..f3cd83b3 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -2808,7 +2808,6 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { volatile TIM_TypeDef *tim = is_second_motor ? TIM8 : TIM1; #endif #else - motor_all_state_t *motor_other = (motor_all_state_t*)&m_motor_1; motor_all_state_t *motor_now = (motor_all_state_t*)&m_motor_1;; m_isr_motor = 1; From 762866745876f2dd34e009c7b2a336705dda86e0 Mon Sep 17 00:00:00 2001 From: Euan Date: Wed, 7 Feb 2024 00:04:15 +0000 Subject: [PATCH 07/10] Refactor raw sampling to improve readability Cleaned up the sensor picking, my brain is happy now! --- motor/mc_interface.c | 32 +++++++++++----- motor/mcpwm.c | 88 ++++++++++++++++++++++++-------------------- motor/mcpwm_foc.c | 67 ++++++++++++++++++--------------- 3 files changed, 109 insertions(+), 78 deletions(-) diff --git a/motor/mc_interface.c b/motor/mc_interface.c index a9bfdfbb..dbaa3cc6 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; @@ -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]; + m_curr1_samples[m_sample_now] = ADC_curr_norm_value[4]; + m_curr2_samples[m_sample_now] = ADC_curr_norm_value[5]; + } 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]; + m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1]; + m_curr2_samples[m_sample_now] = ADC_curr_norm_value[2]; + } m_ph1_samples[m_sample_now] = ADC_V_L1 - zero; m_ph2_samples[m_sample_now] = ADC_V_L2 - zero; @@ -2872,9 +2886,9 @@ static THD_FUNCTION(sample_send_thread, arg) { buffer[index++] = COMM_SAMPLE_PRINT; if (m_sample_raw) { - buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp] / FAC_CURRENT, &index); // TODO: Make this accurate for calibrated measurements. - 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], &index); + buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp], &index); + buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp], &index); buffer_append_float32_auto(buffer, (float)m_ph1_samples[ind_samp], &index); buffer_append_float32_auto(buffer, (float)m_ph2_samples[ind_samp], &index); buffer_append_float32_auto(buffer, (float)m_ph3_samples[ind_samp], &index); diff --git a/motor/mcpwm.c b/motor/mcpwm.c index a171e937..71ea1768 100644 --- a/motor/mcpwm.c +++ b/motor/mcpwm.c @@ -1549,25 +1549,35 @@ void mcpwm_adc_inj_int_handler(void) { } #endif - ADC_curr_norm_value[0] = curr0 * FAC_CURRENT1; - ADC_curr_norm_value[1] = curr1 * FAC_CURRENT2; + // Store raw ADC readings for raw sampling mode. + ADC_curr_raw[0] = curr0; + ADC_curr_raw[1] = curr1; + ADC_curr_raw[2] = curr2; + + // Scale to AMPs using calibrated scaling factors + curr0 *= FAC_CURRENT1; + curr1 *= FAC_CURRENT2; + curr2 *= FAC_CURRENT3; + +#ifndef HW_HAS_3_SHUNTS + curr2 = -(curr0 + curr1); +#endif -#ifdef HW_HAS_3_SHUNTS - ADC_curr_norm_value[2] = curr2 * FAC_CURRENT3; -#else - ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); -#endif + // Store the currents for sampling + ADC_curr_norm_value[0] = curr0; + ADC_curr_norm_value[1] = curr1; + 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) * FAC_CURRENT3; + curr_tot_sample = -(GET_CURRENT3() - curr2_offset) * FAC_CURRENT3; #else - curr_tot_sample = -(GET_CURRENT2() - curr1_offset) * FAC_CURRENT2; + curr_tot_sample = -(GET_CURRENT2() - curr1_offset) * FAC_CURRENT2; #endif } else { - curr_tot_sample = -(GET_CURRENT1() - curr0_offset) * FAC_CURRENT1; + curr_tot_sample = -(GET_CURRENT1() - curr0_offset) * FAC_CURRENT1; } } else { static int detect_now = 0; @@ -1593,52 +1603,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 +1665,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; diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index f3cd83b3..2f22dbc8 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -2945,7 +2945,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { curr2 += GET_CURRENT3_M2(); #endif -// Store ADC readings +// Store raw ADC readings motor_now->m_currents_adc[0] = curr0; motor_now->m_currents_adc[1] = curr1; motor_now->m_currents_adc[2] = curr2; @@ -2955,6 +2955,11 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { curr1 -= conf_now->foc_offsets_current[1]; 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; @@ -2970,13 +2975,10 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { curr1 *= FAC_CURRENT2; curr2 *= FAC_CURRENT3; } - - 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]); + +#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. @@ -2984,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 @@ -3001,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); } } } @@ -3030,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]; - float ib = ADC_curr_norm_value[1 + norm_curr_ofs]; - float ic = ADC_curr_norm_value[2 + norm_curr_ofs]; + + // 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. From 1f3667018f02962e88315d88c924c2d2d43922e3 Mon Sep 17 00:00:00 2001 From: Euan Date: Wed, 7 Feb 2024 01:05:35 +0000 Subject: [PATCH 08/10] Fix compiler errors. --- hwconf/hw.h | 40 +++++++++++++++++++++------------ lispBM/lispif_vesc_extensions.c | 2 +- motor/mc_interface.h | 2 ++ motor/mcpwm.c | 12 +++++----- 4 files changed, 36 insertions(+), 20 deletions(-) diff --git a/hwconf/hw.h b/hwconf/hw.h index ed8181a7..9fb52b27 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -360,40 +360,52 @@ #endif #endif #else + #define ADC_IND_CURR3 0 #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]) + #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() ((float)ADC_Value[ADC_IND_CURR4]) + #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() ((float)ADC_Value[ADC_IND_CURR5]) + #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 + #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() 0 + #define GET_CURRENT3_M2() ((float)ADC_Value[ADC_IND_CURR6]) #endif - #endif + #else + #define GET_CURRENT3_M2() 0 + #define ADC_IND_CURR6 0 + #endif #endif #else #define GET_CURRENT3_M2() 0 + #define ADC_IND_CURR6 0 #endif diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index ac8ee2b0..6a93ed6a 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -2445,7 +2445,7 @@ static lbm_value ext_raw_adc_current(lbm_value *args, lbm_uint argn) { float scale1, scale2, scale3; if(motor == 2) { scale1 = FAC_CURRENT1_M2; scale2 = FAC_CURRENT2_M2; scale3 = FAC_CURRENT3_M2; - } else + } else { scale1 = FAC_CURRENT1; scale2 = FAC_CURRENT2; scale3 = FAC_CURRENT3; } 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 71ea1768..3801e6c7 100644 --- a/motor/mcpwm.c +++ b/motor/mcpwm.c @@ -1552,16 +1552,18 @@ void mcpwm_adc_inj_int_handler(void) { // 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; + curr1 *= FAC_CURRENT2; +#ifdef HW_HAS_3_SHUNTS curr2 *= FAC_CURRENT3; - -#ifndef HW_HAS_3_SHUNTS - curr2 = -(curr0 + curr1); -#endif +#else + int curr2 = -(curr0 + curr1); +#endif // Store the currents for sampling ADC_curr_norm_value[0] = curr0; From e26a2bac38c543b614fce5b50f275098f445e25d Mon Sep 17 00:00:00 2001 From: Euan Date: Wed, 7 Feb 2024 01:30:10 +0000 Subject: [PATCH 09/10] Fix for hw defining index without has_3_shunts defined --- hwconf/hw.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/hwconf/hw.h b/hwconf/hw.h index 9fb52b27..ecb0f990 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -360,7 +360,9 @@ #endif #endif #else - #define ADC_IND_CURR3 0 + #ifndef ADC_IND_CURR3 + #define ADC_IND_CURR3 0 + #endif #define GET_CURRENT3() 0 #endif @@ -405,7 +407,9 @@ #endif #else #define GET_CURRENT3_M2() 0 - #define ADC_IND_CURR6 0 + #ifndef ADC_IND_CURR6 + #define ADC_IND_CURR6 0 + #endif #endif From 5e66bb438755378f02af7cb1cea57e80af51e9dd Mon Sep 17 00:00:00 2001 From: Euan Date: Wed, 7 Feb 2024 22:04:06 +0000 Subject: [PATCH 10/10] Scale sampled data properly Needs to be rescaled back to int for storage, 8* higher resolution so the hw can use oversampling. BLDC can use floats for these variables, since they get changed to floats later anyway. Might as well do it earlier to make the code simpler. --- motor/mc_interface.c | 22 +++++++++++----------- motor/mcpwm.c | 10 +++++----- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/motor/mc_interface.c b/motor/mc_interface.c index dbaa3cc6..635ecd60 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -2164,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]; @@ -2178,9 +2178,9 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { 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]; - m_curr1_samples[m_sample_now] = ADC_curr_norm_value[4]; - m_curr2_samples[m_sample_now] = ADC_curr_norm_value[5]; + 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; @@ -2192,9 +2192,9 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { 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]; - m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1]; - m_curr2_samples[m_sample_now] = ADC_curr_norm_value[2]; + 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; @@ -2895,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], &index); - buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp], &index); - buffer_append_float32_auto(buffer, (float)m_curr2_samples[ind_samp], &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/mcpwm.c b/motor/mcpwm.c index 3801e6c7..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