mirror of https://github.com/rusefi/bldc.git
[Utils] Use convenience macros for deg/rad conversions
This commit is contained in:
Kenn Sebesta
parent
b6eb5852aa
commit
aeb5d74488
|
|
@ -622,10 +622,10 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
|
||||
// Get the values we want
|
||||
last_pitch_angle = pitch_angle;
|
||||
pitch_angle = imu_get_pitch() * 180.0f / M_PI;
|
||||
roll_angle = imu_get_roll() * 180.0f / M_PI;
|
||||
pitch_angle = imu_get_pitch() * RAD2DEG_f;
|
||||
roll_angle = imu_get_roll() * RAD2DEG_f;
|
||||
abs_roll_angle = fabsf(roll_angle);
|
||||
abs_roll_angle_sin = sinf(abs_roll_angle * M_PI / 180.0f);
|
||||
abs_roll_angle_sin = sinf(abs_roll_angle * DEG2RAD_f);
|
||||
imu_get_gyro(gyro);
|
||||
duty_cycle = mc_interface_get_duty_cycle_now();
|
||||
abs_duty_cycle = fabsf(duty_cycle);
|
||||
|
|
|
|||
|
|
@ -873,7 +873,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
|
|||
avg_current /= samples;
|
||||
avg_voltage -= avg_current * res * 2.0;
|
||||
|
||||
*linkage = avg_voltage * 60.0 / (sqrtf(3.0) * 2.0 * M_PI * avg_rpm);
|
||||
*linkage = avg_voltage / (sqrtf(3.0) * (avg_rpm * RPM2RADPS_f));
|
||||
|
||||
mempools_free_mcconf(mcconf);
|
||||
mempools_free_mcconf(mcconf_old);
|
||||
|
|
@ -1065,7 +1065,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
|||
iq_avg /= samples2;
|
||||
id_avg /= samples2;
|
||||
|
||||
float rad_s = rpm_now * ((2.0 * M_PI) / 60.0);
|
||||
float rad_s = rpm_now * RPM2RADPS_f;
|
||||
float v_mag = sqrtf(SQ(vq_avg) + SQ(vd_avg));
|
||||
float i_mag = sqrtf(SQ(iq_avg) + SQ(id_avg));
|
||||
*linkage = (v_mag - (2.0 / 3.0) * res * i_mag) / rad_s - (2.0 / 3.0) * i_mag * ind;
|
||||
|
|
@ -1082,7 +1082,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
|||
float linkage_sum = 0.0;
|
||||
float linkage_samples = 0.0;
|
||||
for (int i = 0;i < 20000;i++) {
|
||||
float rad_s_now = mcpwm_foc_get_rpm_faster() * ((2.0 * M_PI) / 60.0);
|
||||
float rad_s_now = mcpwm_foc_get_rpm_faster() * RPM2RADPS_f;
|
||||
if (fabsf(mcpwm_foc_get_duty_cycle_now()) < 0.01) {
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -548,7 +548,7 @@ float encoder_read_deg(void) {
|
|||
UTILS_LP_FAST(sincos_signal_above_max_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
|
||||
UTILS_LP_FAST(sincos_signal_low_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
|
||||
|
||||
float angle_tmp = utils_fast_atan2(sin, cos) * 180.0 / M_PI;
|
||||
float angle_tmp = utils_fast_atan2(sin, cos) * RAD2DEG_f;
|
||||
UTILS_LP_FAST(angle, angle_tmp, sincos_filter_constant);
|
||||
last_enc_angle = angle;
|
||||
}
|
||||
|
|
|
|||
34
imu/imu.c
34
imu/imu.c
|
|
@ -334,7 +334,7 @@ void imu_get_calibration(float yaw, float *imu_cal) {
|
|||
roll_sample = roll_sample / 250;
|
||||
|
||||
// Set roll rotations to level out roll axis
|
||||
m_settings.rot_roll = -roll_sample * (180 / M_PI);
|
||||
m_settings.rot_roll = -roll_sample * RAD2DEG_f;
|
||||
|
||||
// Rotate gyro offsets to match new IMU orientation
|
||||
float rotation1[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
|
||||
|
|
@ -353,7 +353,7 @@ void imu_get_calibration(float yaw, float *imu_cal) {
|
|||
pitch_sample = pitch_sample / 250;
|
||||
|
||||
// Set pitch rotation to level out pitch axis
|
||||
m_settings.rot_pitch = pitch_sample * (180 / M_PI);
|
||||
m_settings.rot_pitch = pitch_sample * RAD2DEG_f;
|
||||
|
||||
// Rotate imu offsets to match
|
||||
float rotation2[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
|
||||
|
|
@ -445,12 +445,12 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
|
|||
|
||||
// Rotate axes (ZYX)
|
||||
|
||||
float s1 = sinf(m_settings.rot_yaw * M_PI / 180.0);
|
||||
float c1 = cosf(m_settings.rot_yaw * M_PI / 180.0);
|
||||
float s2 = sinf(m_settings.rot_pitch * M_PI / 180.0);
|
||||
float c2 = cosf(m_settings.rot_pitch * M_PI / 180.0);
|
||||
float s3 = sinf(m_settings.rot_roll * M_PI / 180.0);
|
||||
float c3 = cosf(m_settings.rot_roll * M_PI / 180.0);
|
||||
float s1 = sinf(m_settings.rot_yaw * DEG2RAD_f);
|
||||
float c1 = cosf(m_settings.rot_yaw * DEG2RAD_f);
|
||||
float s2 = sinf(m_settings.rot_pitch * DEG2RAD_f);
|
||||
float c2 = cosf(m_settings.rot_pitch * DEG2RAD_f);
|
||||
float s3 = sinf(m_settings.rot_roll * DEG2RAD_f);
|
||||
float c3 = cosf(m_settings.rot_roll * DEG2RAD_f);
|
||||
|
||||
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
|
||||
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
|
||||
|
|
@ -475,9 +475,9 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
|
|||
}
|
||||
|
||||
float gyro_rad[3];
|
||||
gyro_rad[0] = m_gyro[0] * M_PI / 180.0;
|
||||
gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
|
||||
gyro_rad[2] = m_gyro[2] * M_PI / 180.0;
|
||||
gyro_rad[0] = m_gyro[0] * DEG2RAD_f;
|
||||
gyro_rad[1] = m_gyro[1] * DEG2RAD_f;
|
||||
gyro_rad[2] = m_gyro[2] * DEG2RAD_f;
|
||||
|
||||
switch (m_settings.mode) {
|
||||
case AHRS_MODE_MADGWICK:
|
||||
|
|
@ -508,12 +508,12 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
|
|||
|
||||
void rotate(float *input, float *rotation, float *output) {
|
||||
// Rotate imu offsets to match
|
||||
float s1 = sinf(rotation[2] * M_PI / 180.0);
|
||||
float c1 = cosf(rotation[2] * M_PI / 180.0);
|
||||
float s2 = sinf(rotation[1] * M_PI / 180.0);
|
||||
float c2 = cosf(rotation[1] * M_PI / 180.0);
|
||||
float s3 = sinf(rotation[0] * M_PI / 180.0);
|
||||
float c3 = cosf(rotation[0] * M_PI / 180.0);
|
||||
float s1 = sinf(rotation[2] * DEG2RAD_f);
|
||||
float c1 = cosf(rotation[2] * DEG2RAD_f);
|
||||
float s2 = sinf(rotation[1] * DEG2RAD_f);
|
||||
float c2 = cosf(rotation[1] * DEG2RAD_f);
|
||||
float s3 = sinf(rotation[0] * DEG2RAD_f);
|
||||
float c3 = cosf(rotation[0] * DEG2RAD_f);
|
||||
|
||||
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
|
||||
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;
|
||||
|
|
|
|||
6
mcpwm.c
6
mcpwm.c
|
|
@ -727,7 +727,7 @@ float mcpwm_get_switching_frequency_now(void) {
|
|||
*/
|
||||
float mcpwm_get_rpm(void) {
|
||||
if (conf->motor_type == MOTOR_TYPE_DC) {
|
||||
return m_pll_speed / ((2.0 * M_PI) / 60.0);
|
||||
return m_pll_speed * RADPS2RPM_f;
|
||||
} else {
|
||||
return direction ? rpm_now : -rpm_now;
|
||||
}
|
||||
|
|
@ -2110,7 +2110,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
|||
if (encoder_is_configured()) {
|
||||
float pos = encoder_read_deg();
|
||||
run_pid_control_pos(1.0 / switching_frequency_now, pos);
|
||||
pll_run(-pos * M_PI / 180.0, 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
|
||||
pll_run(-pos * DEG2RAD_f, 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
|
||||
}
|
||||
|
||||
last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
|
||||
|
|
@ -2164,7 +2164,7 @@ float mcpwm_get_detect_pos(void) {
|
|||
v2 /= amp;
|
||||
|
||||
float ph[1];
|
||||
ph[0] = asinf(v0) * 180.0 / M_PI;
|
||||
ph[0] = asinf(v0) * RAD2DEG_f;
|
||||
|
||||
float res = ph[0];
|
||||
if (v1 < v2) {
|
||||
|
|
|
|||
69
mcpwm_foc.c
69
mcpwm_foc.c
|
|
@ -949,7 +949,7 @@ void mcpwm_foc_set_openloop(float current, float rpm) {
|
|||
|
||||
motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP;
|
||||
motor_now()->m_iq_set = current;
|
||||
motor_now()->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
|
||||
motor_now()->m_openloop_speed = rpm * RPM2RADPS_f;
|
||||
|
||||
if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
|
||||
return;
|
||||
|
|
@ -976,7 +976,7 @@ void mcpwm_foc_set_openloop_phase(float current, float phase) {
|
|||
motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
|
||||
motor_now()->m_iq_set = current;
|
||||
|
||||
motor_now()->m_openloop_phase = phase * M_PI / 180.0;
|
||||
motor_now()->m_openloop_phase = phase * DEG2RAD_f;
|
||||
utils_norm_angle_rad((float*)&motor_now()->m_openloop_phase);
|
||||
|
||||
if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
|
||||
|
|
@ -1051,7 +1051,7 @@ void mcpwm_foc_get_voltage_offsets_undriven(
|
|||
void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
|
||||
motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY;
|
||||
motor_now()->m_duty_cycle_set = dutyCycle;
|
||||
motor_now()->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
|
||||
motor_now()->m_openloop_speed = rpm * RPM2RADPS_f;
|
||||
|
||||
if (motor_now()->m_state != MC_STATE_RUNNING) {
|
||||
motor_now()->m_state = MC_STATE_RUNNING;
|
||||
|
|
@ -1070,7 +1070,7 @@ void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
|
|||
void mcpwm_foc_set_openloop_duty_phase(float dutyCycle, float phase) {
|
||||
motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE;
|
||||
motor_now()->m_duty_cycle_set = dutyCycle;
|
||||
motor_now()->m_openloop_phase = phase * M_PI / 180.0;
|
||||
motor_now()->m_openloop_phase = phase * DEG2RAD_f;
|
||||
utils_norm_angle_rad((float*)&motor_now()->m_openloop_phase);
|
||||
|
||||
if (motor_now()->m_state != MC_STATE_RUNNING) {
|
||||
|
|
@ -1197,22 +1197,22 @@ mc_state mcpwm_foc_get_state_motor(bool is_second_motor) {
|
|||
* The RPM value.
|
||||
*/
|
||||
float mcpwm_foc_get_rpm(void) {
|
||||
return motor_now()->m_motor_state.speed_rad_s / ((2.0 * M_PI) / 60.0);
|
||||
// return motor_now()->m_speed_est_fast / ((2.0 * M_PI) / 60.0);
|
||||
return motor_now()->m_motor_state.speed_rad_s * RADPS2RPM_f;
|
||||
// return motor_now()->m_speed_est_fast * RADPS2RPM_f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Same as above, but uses the fast and noisier estimator.
|
||||
*/
|
||||
float mcpwm_foc_get_rpm_fast(void) {
|
||||
return motor_now()->m_speed_est_fast / ((2.0 * M_PI) / 60.0);
|
||||
return motor_now()->m_speed_est_fast * RADPS2RPM_f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Same as above, but uses the faster and noisier estimator.
|
||||
*/
|
||||
float mcpwm_foc_get_rpm_faster(void) {
|
||||
return motor_now()->m_speed_est_faster / ((2.0 * M_PI) / 60.0);
|
||||
return motor_now()->m_speed_est_faster * RADPS2RPM_f;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -1412,7 +1412,7 @@ int mcpwm_foc_get_tachometer_abs_value(bool reset) {
|
|||
* The phase angle in degrees.
|
||||
*/
|
||||
float mcpwm_foc_get_phase(void) {
|
||||
float angle = motor_now()->m_motor_state.phase * (180.0 / M_PI);
|
||||
float angle = motor_now()->m_motor_state.phase * RAD2DEG_f;
|
||||
utils_norm_angle(&angle);
|
||||
return angle;
|
||||
}
|
||||
|
|
@ -1424,7 +1424,7 @@ float mcpwm_foc_get_phase(void) {
|
|||
* The phase angle in degrees.
|
||||
*/
|
||||
float mcpwm_foc_get_phase_observer(void) {
|
||||
float angle = motor_now()->m_phase_now_observer * (180.0 / M_PI);
|
||||
float angle = motor_now()->m_phase_now_observer * RAD2DEG_f;
|
||||
utils_norm_angle(&angle);
|
||||
return angle;
|
||||
}
|
||||
|
|
@ -1436,7 +1436,7 @@ float mcpwm_foc_get_phase_observer(void) {
|
|||
* The phase angle in degrees.
|
||||
*/
|
||||
float mcpwm_foc_get_phase_encoder(void) {
|
||||
float angle = motor_now()->m_phase_now_encoder * (180.0 / M_PI);
|
||||
float angle = motor_now()->m_phase_now_encoder * RAD2DEG_f;
|
||||
utils_norm_angle(&angle);
|
||||
return angle;
|
||||
}
|
||||
|
|
@ -1556,7 +1556,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(diff * RAD2DEG_f));
|
||||
}
|
||||
|
||||
if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
||||
|
|
@ -1584,7 +1584,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(diff * RAD2DEG_f));
|
||||
}
|
||||
|
||||
if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
|
||||
|
|
@ -1592,7 +1592,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
}
|
||||
}
|
||||
|
||||
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
|
||||
float diff = atan2f(s_sum, c_sum) * RAD2DEG_f;
|
||||
*inverted = diff < 0.0;
|
||||
*ratio = roundf(((2.0 / 3.0) * 180.0) / fabsf(diff));
|
||||
|
||||
|
|
@ -1636,7 +1636,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(angle_diff * RAD2DEG_f));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1658,11 +1658,11 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
|||
c_sum += c;
|
||||
|
||||
if (print) {
|
||||
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
|
||||
commands_printf("%.2f", (double)(angle_diff * RAD2DEG_f));
|
||||
}
|
||||
}
|
||||
|
||||
*offset = atan2f(s_sum, c_sum) * 180.0 / M_PI;
|
||||
*offset = atan2f(s_sum, c_sum) * RAD2DEG_f;
|
||||
|
||||
if (print) {
|
||||
commands_printf("Avg: %.2f", (double)*offset);
|
||||
|
|
@ -2063,7 +2063,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
|||
// Forwards
|
||||
for (int i = 0;i < 3;i++) {
|
||||
for (int j = 0;j < 360;j++) {
|
||||
motor->m_phase_now_override = (float)j * M_PI / 180.0;
|
||||
motor->m_phase_now_override = j * DEG2RAD_f;
|
||||
chThdSleepMilliseconds(5);
|
||||
|
||||
int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
|
||||
|
|
@ -2078,7 +2078,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
|||
// Reverse
|
||||
for (int i = 0;i < 3;i++) {
|
||||
for (int j = 360;j >= 0;j--) {
|
||||
motor->m_phase_now_override = (float)j * M_PI / 180.0;
|
||||
motor->m_phase_now_override = j * DEG2RAD_f;
|
||||
chThdSleepMilliseconds(5);
|
||||
|
||||
int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
|
||||
|
|
@ -2105,7 +2105,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
|||
int fails = 0;
|
||||
for(int i = 0;i < 8;i++) {
|
||||
if (hall_iterations[i] > 30) {
|
||||
float ang = atan2f(sin_hall[i], cos_hall[i]) * 180.0 / M_PI;
|
||||
float ang = atan2f(sin_hall[i], cos_hall[i]) * RAD2DEG_f;
|
||||
utils_norm_angle(&ang);
|
||||
hall_table[i] = (uint8_t)(ang * 200.0 / 360.0);
|
||||
} else {
|
||||
|
|
@ -2618,7 +2618,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
|||
phase_tmp *= conf_now->foc_encoder_ratio;
|
||||
phase_tmp -= conf_now->foc_encoder_offset;
|
||||
utils_norm_angle((float*)&phase_tmp);
|
||||
motor_now->m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
|
||||
motor_now->m_phase_now_encoder = phase_tmp * DEG2RAD_f;
|
||||
}
|
||||
|
||||
const float phase_diff = utils_angle_difference_rad(motor_now->m_motor_state.phase, motor_now->m_phase_before);
|
||||
|
|
@ -2929,11 +2929,12 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
|||
case FOC_SENSOR_MODE_HFI:
|
||||
case FOC_SENSOR_MODE_HFI_START:{
|
||||
motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
|
||||
if (fabsf(motor_now->m_pll_speed * (60.0 / (2.0 * M_PI))) < (conf_now->foc_sl_erpm_hfi * 1.1)) {
|
||||
if (fabsf(motor_now->m_pll_speed * RADPS2RPM_f) < (conf_now->foc_sl_erpm_hfi * 1.1)) {
|
||||
motor_now->m_hfi.est_done_cnt = 0;
|
||||
motor_now->m_hfi.flip_cnt = 0;
|
||||
}
|
||||
} break;
|
||||
|
||||
}
|
||||
|
||||
utils_fast_sincos_better(motor_now->m_motor_state.phase,
|
||||
|
|
@ -3034,7 +3035,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
|||
angle_now = enc_ang;
|
||||
}
|
||||
} else {
|
||||
angle_now = motor_now->m_motor_state.phase * (180.0 / M_PI);
|
||||
angle_now = motor_now->m_motor_state.phase * RAD2DEG_f;
|
||||
}
|
||||
|
||||
utils_norm_angle(&angle_now);
|
||||
|
|
@ -3227,16 +3228,16 @@ static void timer_update(volatile motor_all_state_t *motor, float dt) {
|
|||
|
||||
float add_min_speed = 0.0;
|
||||
if (motor->m_motor_state.duty_now > 0.0) {
|
||||
add_min_speed = ((openloop_rpm * 2.0 * M_PI) / 60.0) * dt;
|
||||
add_min_speed = (openloop_rpm * RPM2RADPS_f) * dt;
|
||||
} else {
|
||||
add_min_speed = -((openloop_rpm * 2.0 * M_PI) / 60.0) * dt;
|
||||
add_min_speed = -(openloop_rpm * RPM2RADPS_f) * dt;
|
||||
}
|
||||
|
||||
// Open loop encoder angle for when the index is not found
|
||||
motor->m_phase_now_encoder_no_index += add_min_speed;
|
||||
utils_norm_angle_rad((float*)&motor->m_phase_now_encoder_no_index);
|
||||
|
||||
if (fabsf(motor->m_pll_speed) < ((openloop_rpm_max * 2.0 * M_PI) / 60.0) &&
|
||||
if (fabsf(motor->m_pll_speed) < (openloop_rpm_max * RPM2RADPS_f) &&
|
||||
motor->m_min_rpm_hyst_timer < motor->m_conf->foc_sl_openloop_hyst) {
|
||||
motor->m_min_rpm_hyst_timer += dt;
|
||||
} else if (motor->m_min_rpm_hyst_timer > 0.0) {
|
||||
|
|
@ -3355,7 +3356,7 @@ static THD_FUNCTION(timer_thread, arg) {
|
|||
}
|
||||
|
||||
static void hfi_update(volatile motor_all_state_t *motor) {
|
||||
float rpm_abs = fabsf(motor->m_speed_est_fast * (60.0 / (2.0 * M_PI)));
|
||||
float rpm_abs = fabsf(motor->m_speed_est_fast * RADPS2RPM_f);
|
||||
|
||||
if (rpm_abs > motor->m_conf->foc_sl_erpm_hfi) {
|
||||
motor->m_hfi.angle = motor->m_phase_now_observer;
|
||||
|
|
@ -3655,7 +3656,7 @@ static void control_current(volatile motor_all_state_t *motor, float dt) {
|
|||
float s = state_m->phase_sin;
|
||||
float c = state_m->phase_cos;
|
||||
|
||||
float abs_rpm = fabsf(motor->m_speed_est_fast * 60 / (2 * M_PI));
|
||||
float abs_rpm = fabsf(motor->m_speed_est_fast * RADPS2RPM_f);
|
||||
|
||||
bool do_hfi = (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI ||
|
||||
conf_now->foc_sensor_mode == FOC_SENSOR_MODE_HFI_START) &&
|
||||
|
|
@ -4449,7 +4450,7 @@ static void start_pwm_hw(volatile motor_all_state_t *motor) {
|
|||
|
||||
static float correct_encoder(float obs_angle, float enc_angle, float speed,
|
||||
float sl_erpm, volatile motor_all_state_t *motor) {
|
||||
float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
|
||||
float rpm_abs = fabsf(speed * RADPS2RPM_f);
|
||||
|
||||
// Hysteresis 5 % of total speed
|
||||
float hyst = sl_erpm * 0.05;
|
||||
|
|
@ -4471,8 +4472,8 @@ static float correct_hall(float angle, float dt, volatile motor_all_state_t *mot
|
|||
motor->m_hall_dt_diff_now += dt;
|
||||
|
||||
float rad_per_sec = (M_PI / 3.0) / motor->m_hall_dt_diff_last;
|
||||
float rpm_abs_fast = fabsf(motor->m_speed_est_fast / ((2.0 * M_PI) / 60.0));
|
||||
float rpm_abs_hall = fabsf(rad_per_sec / ((2.0 * M_PI) / 60.0));
|
||||
float rpm_abs_fast = fabsf(motor->m_speed_est_fast * RADPS2RPM_f);
|
||||
float rpm_abs_hall = fabsf(rad_per_sec * RADPS2RPM_f);
|
||||
|
||||
// Hysteresis 5 % of total speed
|
||||
float hyst = conf_now->foc_sl_erpm * 0.1;
|
||||
|
|
@ -4491,7 +4492,7 @@ static float correct_hall(float angle, float dt, volatile motor_all_state_t *mot
|
|||
|
||||
// Only override the observer if the hall sensor value is valid.
|
||||
if (ang_hall_int < 201) {
|
||||
float ang_hall_now = (((float)ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
|
||||
float ang_hall_now = (((float)ang_hall_int / 200.0) * 360.0) * DEG2RAD_f;
|
||||
|
||||
if (motor->m_ang_hall_int_prev < 0) {
|
||||
// Previous angle not valid
|
||||
|
|
@ -4522,12 +4523,12 @@ static float correct_hall(float angle, float dt, volatile motor_all_state_t *mot
|
|||
// A transition was just made. The angle is in the middle of the new and old angle.
|
||||
int ang_avg = motor->m_ang_hall_int_prev + diff / 2;
|
||||
ang_avg %= 200;
|
||||
motor->m_ang_hall = (((float)ang_avg / 200.0) * 360.0) * M_PI / 180.0;
|
||||
motor->m_ang_hall = (((float)ang_avg / 200.0) * 360.0) * DEG2RAD_f;
|
||||
}
|
||||
|
||||
motor->m_ang_hall_int_prev = ang_hall_int;
|
||||
|
||||
if (((60.0 / (2.0 * M_PI)) * ((M_PI / 3.0) /
|
||||
if ((RADPS2RPM_f * ((M_PI / 3.0) /
|
||||
fmaxf(fabsf(motor->m_hall_dt_diff_now),
|
||||
fabsf(motor->m_hall_dt_diff_last)))) < conf_now->foc_hall_interp_erpm) {
|
||||
// Don't interpolate on very low speed, just use the closest hall sensor. The reason is that we might
|
||||
|
|
|
|||
|
|
@ -483,7 +483,7 @@ void terminal_process_string(char *str) {
|
|||
rpm_avg /= samples;
|
||||
iq_avg /= samples;
|
||||
|
||||
float linkage = (vq_avg - res * iq_avg) / (rpm_avg * ((2.0 * M_PI) / 60.0));
|
||||
float linkage = (vq_avg - res * iq_avg) / (rpm_avg * RPM2RADPS_f);
|
||||
|
||||
commands_printf("Flux linkage: %.7f\n", (double)linkage);
|
||||
} else {
|
||||
|
|
|
|||
7
utils.h
7
utils.h
|
|
@ -81,6 +81,13 @@ const char* utils_hw_type_to_string(HW_TYPE hw);
|
|||
#define UTILS_IS_NAN(x) ((x) != (x))
|
||||
#define UTILS_NAN_ZERO(x) (x = UTILS_IS_NAN(x) ? 0.0 : x)
|
||||
|
||||
// Handy conversions for radians/degrees and RPM/radians-per-second
|
||||
#define DEG2RAD_f ((float)(M_PI / 180.0))
|
||||
#define RAD2DEG_f ((float)(180.0 / M_PI))
|
||||
#define RPM2RADPS_f ((float)((2.0 * M_PI) / 60.0))
|
||||
#define RADPS2RPM_f ((float)(60.0 / (2.0 * M_PI)))
|
||||
|
||||
|
||||
/**
|
||||
* A simple low pass filter.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -159,7 +159,7 @@ bool virtual_motor_is_connected(void){
|
|||
}
|
||||
|
||||
float virtual_motor_get_angle_deg(void){
|
||||
return (virtual_motor.phi * 180.0 / M_PI);
|
||||
return (virtual_motor.phi * RAD2DEG_f);
|
||||
}
|
||||
|
||||
//Private Functions
|
||||
|
|
@ -214,7 +214,7 @@ static void connect_virtual_motor(float ml , float J, float Vbus){
|
|||
GET_GATE_DRIVER_SUPPLY_VOLTAGE();
|
||||
}
|
||||
#endif
|
||||
virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;
|
||||
virtual_motor.phi = mcpwm_foc_get_phase() * DEG2RAD_f;
|
||||
utils_fast_sincos_better(virtual_motor.phi, (float*)&virtual_motor.sin_phi,
|
||||
(float*)&virtual_motor.cos_phi);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue