mirror of https://github.com/rusefi/bldc.git
Fix NaN-problem after lost tracking on some motors
This commit is contained in:
parent
a8f989eb64
commit
4eec52d5c0
|
@ -216,8 +216,7 @@
|
||||||
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
||||||
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||||
|
|
||||||
// Override dead time. See the stm32f4 reference manual for calculating this value.
|
#define HW_DEAD_TIME_NSEC 1000.0
|
||||||
#define HW_DEAD_TIME_NSEC 660.0
|
|
||||||
|
|
||||||
// Default setting overrides
|
// Default setting overrides
|
||||||
#ifndef MCCONF_L_MIN_VOLTAGE
|
#ifndef MCCONF_L_MIN_VOLTAGE
|
||||||
|
|
|
@ -2765,6 +2765,9 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
||||||
observer_update(motor_now->m_motor_state.v_alpha, motor_now->m_motor_state.v_beta,
|
observer_update(motor_now->m_motor_state.v_alpha, motor_now->m_motor_state.v_beta,
|
||||||
motor_now->m_motor_state.i_alpha, motor_now->m_motor_state.i_beta, dt,
|
motor_now->m_motor_state.i_alpha, motor_now->m_motor_state.i_beta, dt,
|
||||||
&motor_now->m_observer_x1, &motor_now->m_observer_x2, &motor_now->m_phase_now_observer, motor_now);
|
&motor_now->m_observer_x1, &motor_now->m_observer_x2, &motor_now->m_phase_now_observer, motor_now);
|
||||||
|
|
||||||
|
// Compensate from the phase lag caused by the switching frequency. This is important for motors
|
||||||
|
// that run on high ERPM compared to the switching frequency.
|
||||||
motor_now->m_phase_now_observer += motor_now->m_pll_speed * dt * 0.5;
|
motor_now->m_phase_now_observer += motor_now->m_pll_speed * dt * 0.5;
|
||||||
utils_norm_angle_rad((float*)&motor_now->m_phase_now_observer);
|
utils_norm_angle_rad((float*)&motor_now->m_phase_now_observer);
|
||||||
}
|
}
|
||||||
|
@ -3781,8 +3784,8 @@ static void control_current(volatile motor_all_state_t *motor, float dt) {
|
||||||
float dec_bemf = 0.0;
|
float dec_bemf = 0.0;
|
||||||
|
|
||||||
if (motor->m_control_mode < CONTROL_MODE_HANDBRAKE && conf_now->foc_cc_decoupling != FOC_CC_DECOUPLING_DISABLED) {
|
if (motor->m_control_mode < CONTROL_MODE_HANDBRAKE && conf_now->foc_cc_decoupling != FOC_CC_DECOUPLING_DISABLED) {
|
||||||
float lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff / 2.0;
|
float lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff * 0.5;
|
||||||
float ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff / 2.0;
|
float ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff * 0.5;
|
||||||
|
|
||||||
switch (conf_now->foc_cc_decoupling) {
|
switch (conf_now->foc_cc_decoupling) {
|
||||||
case FOC_CC_DECOUPLING_CROSS:
|
case FOC_CC_DECOUPLING_CROSS:
|
||||||
|
|
2
utils.c
2
utils.c
|
@ -323,6 +323,8 @@ float utils_fast_atan2(float y, float x) {
|
||||||
angle = ((0.1963 * rsq) - 0.9817) * r + (3.0 * M_PI / 4.0);
|
angle = ((0.1963 * rsq) - 0.9817) * r + (3.0 * M_PI / 4.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
UTILS_NAN_ZERO(angle);
|
||||||
|
|
||||||
if (y < 0) {
|
if (y < 0) {
|
||||||
return(-angle);
|
return(-angle);
|
||||||
} else {
|
} else {
|
||||||
|
|
Loading…
Reference in New Issue