From aeb5d74488de01f74c2da0a75d7049d06a23da55 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Wed, 13 Oct 2021 11:26:19 -0400 Subject: [PATCH] [Utils] Use convenience macros for deg/rad conversions --- applications/app_balance.c | 6 ++-- conf_general.c | 6 ++-- encoder.c | 2 +- imu/imu.c | 34 +++++++++---------- mcpwm.c | 6 ++-- mcpwm_foc.c | 69 +++++++++++++++++++------------------- terminal.c | 2 +- utils.h | 7 ++++ virtual_motor.c | 4 +-- 9 files changed, 72 insertions(+), 64 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 647ff7fa..5936d436 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -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); diff --git a/conf_general.c b/conf_general.c index 11462eb8..1b36702e 100644 --- a/conf_general.c +++ b/conf_general.c @@ -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; } diff --git a/encoder.c b/encoder.c index 0d37451d..521239c2 100644 --- a/encoder.c +++ b/encoder.c @@ -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; } diff --git a/imu/imu.c b/imu/imu.c index 2c526883..da099113 100644 --- a/imu/imu.c +++ b/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; diff --git a/mcpwm.c b/mcpwm.c index 01388b0f..c82122a4 100644 --- a/mcpwm.c +++ b/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) { diff --git a/mcpwm_foc.c b/mcpwm_foc.c index aa51a081..baafc11a 100644 --- a/mcpwm_foc.c +++ b/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 diff --git a/terminal.c b/terminal.c index 3fc227a3..0153ec90 100644 --- a/terminal.c +++ b/terminal.c @@ -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 { diff --git a/utils.h b/utils.h index 62aa793a..3822ef29 100644 --- a/utils.h +++ b/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. * diff --git a/virtual_motor.c b/virtual_motor.c index 9d7cfdbc..09b06888 100644 --- a/virtual_motor.c +++ b/virtual_motor.c @@ -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);