mirror of https://github.com/rusefi/bldc.git
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:
commit
9499e56348
|
@ -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
123
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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
100
motor/mcpwm.c
100
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)) {
|
||||
|
|
|
@ -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.
|
||||
|
|
Loading…
Reference in New Issue