mirror of https://github.com/rusefi/bldc.git
Avoid numerical instability when utils_map is used over a range that approaches 0
This commit is contained in:
parent
3f15ab00da
commit
2133d21fbc
|
@ -11,6 +11,7 @@
|
|||
* Added HFI start sensor mode.
|
||||
* Added TEMP_SENSOR_KTY84_130.
|
||||
* Major UAVCAN update. See: https://github.com/vedderb/bldc/pull/269
|
||||
* Avoid numerical instability when mapping is done over a narrow range. See: https://github.com/vedderb/bldc/issues/262
|
||||
|
||||
=== FW 5.02 ===
|
||||
* IMU calibration improvement.
|
||||
|
|
|
@ -531,7 +531,7 @@ static THD_FUNCTION(adc_thread, arg) {
|
|||
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
||||
|
||||
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
||||
if (config.tc) {
|
||||
if (config.tc && config.tc_max_diff > 1.0) {
|
||||
float rpm_tmp = msg->rpm;
|
||||
if (is_reverse) {
|
||||
rpm_tmp = -rpm_tmp;
|
||||
|
|
|
@ -504,7 +504,7 @@ static THD_FUNCTION(output_thread, arg) {
|
|||
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
||||
bool is_braking = (current > 0.0 && msg->duty < 0.0) || (current < 0.0 && msg->duty > 0.0);
|
||||
|
||||
if (config.tc && !is_braking) {
|
||||
if (config.tc && config.tc_max_diff > 1.0 && !is_braking) {
|
||||
float rpm_tmp = fabsf(msg->rpm);
|
||||
|
||||
float diff = rpm_tmp - rpm_lowest;
|
||||
|
@ -520,7 +520,7 @@ static THD_FUNCTION(output_thread, arg) {
|
|||
|
||||
bool is_braking = (current > 0.0 && duty_now < 0.0) || (current < 0.0 && duty_now > 0.0);
|
||||
|
||||
if (config.tc && !is_braking) {
|
||||
if (config.tc && config.tc_max_diff > 1.0 && !is_braking) {
|
||||
float diff = rpm_local - rpm_lowest;
|
||||
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
|
||||
if (fabsf(current_out) < mcconf->cc_min_current) {
|
||||
|
|
|
@ -194,8 +194,18 @@ static THD_FUNCTION(pas_thread, arg) {
|
|||
output = 0.0;
|
||||
break;
|
||||
case PAS_CTRL_TYPE_CADENCE:
|
||||
output = utils_map(pedal_rpm, config.pedal_rpm_start, config.pedal_rpm_end, 0.0, config.current_scaling);
|
||||
utils_truncate_number(&output, 0.0, config.current_scaling);
|
||||
// NOTE: If the limits are the same a numerical instability is approached, so in that case
|
||||
// just use on/off control (which is what setting the limits to the same value essentially means).
|
||||
if (config.pedal_rpm_end > (config.pedal_rpm_start + 1.0)) {
|
||||
output = utils_map(pedal_rpm, config.pedal_rpm_start, config.pedal_rpm_end, 0.0, config.current_scaling);
|
||||
utils_truncate_number(&output, 0.0, config.current_scaling);
|
||||
} else {
|
||||
if (pedal_rpm > config.pedal_rpm_end) {
|
||||
output = config.current_scaling;
|
||||
} else {
|
||||
output = 0.0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
|
@ -495,7 +495,7 @@ static THD_FUNCTION(ppm_thread, arg) {
|
|||
|
||||
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
||||
//Traction Control - Applied to slaves except if a fault has occured on the local VESC (undriven wheel may generate fake RPM)
|
||||
if (config.tc && !autoTCdisengaged) {
|
||||
if (config.tc && config.tc_max_diff > 1.0 && !autoTCdisengaged) {
|
||||
float rpm_tmp = msg->rpm;
|
||||
if (is_reverse) {
|
||||
rpm_tmp = -rpm_tmp;
|
||||
|
@ -513,7 +513,7 @@ static THD_FUNCTION(ppm_thread, arg) {
|
|||
}
|
||||
}
|
||||
//Traction Control - Applying locally
|
||||
if (config.tc) {
|
||||
if (config.tc && config.tc_max_diff > 1.0) {
|
||||
float diff = rpm_local - rpm_lowest;
|
||||
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
|
||||
if (current_out < mcconf->cc_min_current) {
|
||||
|
|
8
bms.c
8
bms.c
|
@ -286,9 +286,9 @@ void bms_update_limits(float *i_in_min, float *i_in_max,
|
|||
if (UTILS_AGE_S(m_stat_temp_max.rx_time) < MAX_CAN_AGE_SEC) {
|
||||
float temp = m_stat_temp_max.t_cell_max;
|
||||
|
||||
if (temp < m_conf.t_limit_start) {
|
||||
if (temp < (m_conf.t_limit_start + 0.1)) {
|
||||
// OK
|
||||
} else if (temp > m_conf.t_limit_end) {
|
||||
} else if (temp > (m_conf.t_limit_end - 0.1)) {
|
||||
i_in_min_bms = 0.0;
|
||||
i_in_max_bms = 0.0;
|
||||
// Maybe add fault code?
|
||||
|
@ -317,9 +317,9 @@ void bms_update_limits(float *i_in_min, float *i_in_max,
|
|||
if (UTILS_AGE_S(m_stat_soc_min.rx_time) < MAX_CAN_AGE_SEC) {
|
||||
float soc = m_stat_soc_min.soc;
|
||||
|
||||
if (soc > m_conf.soc_limit_start) {
|
||||
if (soc > (m_conf.soc_limit_start - 0.001)) {
|
||||
// OK
|
||||
} else if (soc < m_conf.soc_limit_end) {
|
||||
} else if (soc < (m_conf.soc_limit_end + 0.001)) {
|
||||
i_in_max_bms = 0.0;
|
||||
} else {
|
||||
i_in_max_bms = utils_map(soc, m_conf.soc_limit_start,
|
||||
|
|
12
commands.c
12
commands.c
|
@ -1424,6 +1424,18 @@ void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
|
|||
}
|
||||
|
||||
#ifndef DISABLE_HW_LIMITS
|
||||
|
||||
// TODO: Maybe truncate values that get close to numerical instabilities when set
|
||||
// close to each other, such as
|
||||
//
|
||||
// conf->l_temp_motor_start, conf->l_temp_motor_end
|
||||
// and
|
||||
// conf->l_temp_fet_start, conf->l_temp_fet_end
|
||||
//
|
||||
// A division by 0 is avoided in the code, but getting close can still make things
|
||||
// oscillate. At the moment we leave the responsibility of setting sane values
|
||||
// to the user.
|
||||
|
||||
#ifdef HW_LIM_CURRENT
|
||||
utils_truncate_number(&mcconf->l_current_max, HW_LIM_CURRENT);
|
||||
utils_truncate_number(&mcconf->l_current_min, HW_LIM_CURRENT);
|
||||
|
|
|
@ -24,7 +24,7 @@
|
|||
#define FW_VERSION_MAJOR 5
|
||||
#define FW_VERSION_MINOR 03
|
||||
// Set to 0 for building a release and iterate during beta test builds
|
||||
#define FW_TEST_VERSION_NUMBER 15
|
||||
#define FW_TEST_VERSION_NUMBER 16
|
||||
|
||||
#include "datatypes.h"
|
||||
|
||||
|
|
|
@ -1935,9 +1935,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
// Temperature MOSFET
|
||||
float lo_min_mos = l_current_min_tmp;
|
||||
float lo_max_mos = l_current_max_tmp;
|
||||
if (motor->m_temp_fet < conf->l_temp_fet_start) {
|
||||
if (motor->m_temp_fet < (conf->l_temp_fet_start + 0.1)) {
|
||||
// Keep values
|
||||
} else if (motor->m_temp_fet > conf->l_temp_fet_end) {
|
||||
} else if (motor->m_temp_fet > (conf->l_temp_fet_end - 0.1)) {
|
||||
lo_min_mos = 0.0;
|
||||
lo_max_mos = 0.0;
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET, !is_motor_1, false);
|
||||
|
@ -1961,9 +1961,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
// Temperature MOTOR
|
||||
float lo_min_mot = l_current_min_tmp;
|
||||
float lo_max_mot = l_current_max_tmp;
|
||||
if (motor->m_temp_motor < conf->l_temp_motor_start) {
|
||||
if (motor->m_temp_motor < (conf->l_temp_motor_start + 0.1)) {
|
||||
// Keep values
|
||||
} else if (motor->m_temp_motor > conf->l_temp_motor_end) {
|
||||
} else if (motor->m_temp_motor > (conf->l_temp_motor_end - 0.1)) {
|
||||
lo_min_mot = 0.0;
|
||||
lo_max_mot = 0.0;
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_MOTOR, !is_motor_1, false);
|
||||
|
@ -1992,9 +1992,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
const float temp_motor_accel_end = utils_map(conf->l_temp_accel_dec, 0.0, 1.0, conf->l_temp_motor_end, 25.0);
|
||||
|
||||
float lo_fet_temp_accel = 0.0;
|
||||
if (motor->m_temp_fet < temp_fet_accel_start) {
|
||||
if (motor->m_temp_fet < (temp_fet_accel_start + 0.1)) {
|
||||
lo_fet_temp_accel = l_current_max_tmp;
|
||||
} else if (motor->m_temp_fet > temp_fet_accel_end) {
|
||||
} else if (motor->m_temp_fet > (temp_fet_accel_end - 0.1)) {
|
||||
lo_fet_temp_accel = 0.0;
|
||||
} else {
|
||||
lo_fet_temp_accel = utils_map(motor->m_temp_fet, temp_fet_accel_start,
|
||||
|
@ -2002,9 +2002,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
}
|
||||
|
||||
float lo_motor_temp_accel = 0.0;
|
||||
if (motor->m_temp_motor < temp_motor_accel_start) {
|
||||
if (motor->m_temp_motor < (temp_motor_accel_start + 0.1)) {
|
||||
lo_motor_temp_accel = l_current_max_tmp;
|
||||
} else if (motor->m_temp_motor > temp_motor_accel_end) {
|
||||
} else if (motor->m_temp_motor > (temp_motor_accel_end - 0.1)) {
|
||||
lo_motor_temp_accel = 0.0;
|
||||
} else {
|
||||
lo_motor_temp_accel = utils_map(motor->m_temp_motor, temp_motor_accel_start,
|
||||
|
@ -2015,9 +2015,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
float lo_max_rpm = 0.0;
|
||||
const float rpm_pos_cut_start = conf->l_max_erpm * conf->l_erpm_start;
|
||||
const float rpm_pos_cut_end = conf->l_max_erpm;
|
||||
if (rpm_now < rpm_pos_cut_start) {
|
||||
if (rpm_now < (rpm_pos_cut_start + 0.1)) {
|
||||
lo_max_rpm = l_current_max_tmp;
|
||||
} else if (rpm_now > rpm_pos_cut_end) {
|
||||
} else if (rpm_now > (rpm_pos_cut_end - 0.1)) {
|
||||
lo_max_rpm = 0.0;
|
||||
} else {
|
||||
lo_max_rpm = utils_map(rpm_now, rpm_pos_cut_start, rpm_pos_cut_end, l_current_max_tmp, 0.0);
|
||||
|
@ -2027,9 +2027,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
float lo_min_rpm = 0.0;
|
||||
const float rpm_neg_cut_start = conf->l_min_erpm * conf->l_erpm_start;
|
||||
const float rpm_neg_cut_end = conf->l_min_erpm;
|
||||
if (rpm_now > rpm_neg_cut_start) {
|
||||
if (rpm_now > (rpm_neg_cut_start - 0.1)) {
|
||||
lo_min_rpm = l_current_max_tmp;
|
||||
} else if (rpm_now < rpm_neg_cut_end) {
|
||||
} else if (rpm_now < (rpm_neg_cut_end + 0.1)) {
|
||||
lo_min_rpm = 0.0;
|
||||
} else {
|
||||
lo_min_rpm = utils_map(rpm_now, rpm_neg_cut_start, rpm_neg_cut_end, l_current_max_tmp, 0.0);
|
||||
|
@ -2037,10 +2037,11 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
|
||||
// Duty max
|
||||
float lo_max_duty = 0.0;
|
||||
if (duty_now_abs < conf->l_duty_start) {
|
||||
if (duty_now_abs < (conf->l_duty_start * conf->l_max_duty) || conf->l_duty_start > 0.99) {
|
||||
lo_max_duty = l_current_max_tmp;
|
||||
} else {
|
||||
lo_max_duty = utils_map(duty_now_abs, conf->l_duty_start, conf->l_max_duty, l_current_max_tmp, 0.0);
|
||||
lo_max_duty = utils_map(duty_now_abs, (conf->l_duty_start * conf->l_max_duty),
|
||||
conf->l_max_duty, l_current_max_tmp, 0.0);
|
||||
}
|
||||
|
||||
float lo_max = utils_min_abs(lo_max_mos, lo_max_mot);
|
||||
|
@ -2065,9 +2066,9 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
|
||||
// Battery cutoff
|
||||
float lo_in_max_batt = 0.0;
|
||||
if (v_in > conf->l_battery_cut_start) {
|
||||
if (v_in > (conf->l_battery_cut_start - 0.1)) {
|
||||
lo_in_max_batt = conf->l_in_current_max;
|
||||
} else if (v_in < conf->l_battery_cut_end) {
|
||||
} else if (v_in < (conf->l_battery_cut_end + 0.1)) {
|
||||
lo_in_max_batt = 0.0;
|
||||
} else {
|
||||
lo_in_max_batt = utils_map(v_in, conf->l_battery_cut_start,
|
||||
|
|
22
mcpwm_foc.c
22
mcpwm_foc.c
|
@ -3535,15 +3535,17 @@ static void control_current(volatile motor_all_state_t *motor, float dt) {
|
|||
UTILS_LP_FAST(state_m->iq_filter, state_m->iq, conf_now->foc_current_filter_const);
|
||||
|
||||
float d_gain_scale = 1.0;
|
||||
float max_mod_norm = fabsf(state_m->duty_now / max_duty);
|
||||
if (max_duty < 0.01) {
|
||||
max_mod_norm = 1.0;
|
||||
}
|
||||
if (max_mod_norm > conf_now->foc_d_gain_scale_start) {
|
||||
d_gain_scale = utils_map(max_mod_norm, conf_now->foc_d_gain_scale_start, 1.0,
|
||||
1.0, conf_now->foc_d_gain_scale_max_mod);
|
||||
if (d_gain_scale < conf_now->foc_d_gain_scale_max_mod) {
|
||||
d_gain_scale = conf_now->foc_d_gain_scale_max_mod;
|
||||
if (conf_now->foc_d_gain_scale_start < 0.99) {
|
||||
float max_mod_norm = fabsf(state_m->duty_now / max_duty);
|
||||
if (max_duty < 0.01) {
|
||||
max_mod_norm = 1.0;
|
||||
}
|
||||
if (max_mod_norm > conf_now->foc_d_gain_scale_start) {
|
||||
d_gain_scale = utils_map(max_mod_norm, conf_now->foc_d_gain_scale_start, 1.0,
|
||||
1.0, conf_now->foc_d_gain_scale_max_mod);
|
||||
if (d_gain_scale < conf_now->foc_d_gain_scale_max_mod) {
|
||||
d_gain_scale = conf_now->foc_d_gain_scale_max_mod;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -3655,7 +3657,7 @@ static void control_current(volatile motor_all_state_t *motor, float dt) {
|
|||
if (motor->m_hfi.est_done_cnt < conf_now->foc_hfi_start_samples) {
|
||||
hfi_voltage = conf_now->foc_hfi_voltage_start;
|
||||
} else {
|
||||
hfi_voltage = utils_map(fabsf(state_m->iq), 0.0, conf_now->l_current_max,
|
||||
hfi_voltage = utils_map(fabsf(state_m->iq), -0.01, conf_now->l_current_max,
|
||||
conf_now->foc_hfi_voltage_run, conf_now->foc_hfi_voltage_max);
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue