rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #705 from TechAUmNu/Calibrated-currents 

Add ability to calibrate current sensors, simplify ifdefs in foc, higher resolution sampled current
This commit is contained in:
Benjamin Vedder 2024-02-23 06:52:49 +01:00 committed by GitHub
parent d61bde5841 5e66bb4387
commit 9499e56348
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 253 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
conf_general.h
View File

@ -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 )

123
hwconf/hw.h
View File

@ -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

15
lispBM/lispif_vesc_extensions.c
View File

@ -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;
}
}

36
motor/mc_interface.c
View File

@ -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);

2
motor/mc_interface.h
View File

@ -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

98
motor/mcpwm.c
View File

@ -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
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)) {

119
motor/mcpwm_foc.c
View File

@ -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
#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
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.