Skip to content

Commit bec06ac

Browse files
kubark42kubark42
committed
[Utils] Change convenience multiplication factors into convenience MACROs
1 parent aeb5d74 commit bec06ac

9 files changed

+73
-68
lines changed

‎applications/app_balance.c

+3-3
Original file line numberDiff line numberDiff line change
@@ -622,10 +622,10 @@ static THD_FUNCTION(balance_thread, arg) {
622622

623623
// Get the values we want
624624
last_pitch_angle = pitch_angle;
625-
pitch_angle = imu_get_pitch() * RAD2DEG_f;
626-
roll_angle = imu_get_roll() * RAD2DEG_f;
625+
pitch_angle = RAD2DEG_f(imu_get_pitch());
626+
roll_angle = RAD2DEG_f(imu_get_roll());
627627
abs_roll_angle = fabsf(roll_angle);
628-
abs_roll_angle_sin = sinf(abs_roll_angle * DEG2RAD_f);
628+
abs_roll_angle_sin = sinf(DEG2RAD_f(abs_roll_angle));
629629
imu_get_gyro(gyro);
630630
duty_cycle = mc_interface_get_duty_cycle_now();
631631
abs_duty_cycle = fabsf(duty_cycle);

‎conf_general.c

+3-3
Original file line numberDiff line numberDiff line change
@@ -873,7 +873,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
873873
avg_current /= samples;
874874
avg_voltage -= avg_current * res * 2.0;
875875

876-
*linkage = avg_voltage / (sqrtf(3.0) * (avg_rpm * RPM2RADPS_f));
876+
*linkage = avg_voltage / (sqrtf(3.0) * RPM2RADPS_f(avg_rpm));
877877

878878
mempools_free_mcconf(mcconf);
879879
mempools_free_mcconf(mcconf_old);
@@ -1065,7 +1065,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
10651065
iq_avg /= samples2;
10661066
id_avg /= samples2;
10671067

1068-
float rad_s = rpm_now * RPM2RADPS_f;
1068+
float rad_s = RPM2RADPS_f(rpm_now);
10691069
float v_mag = sqrtf(SQ(vq_avg) + SQ(vd_avg));
10701070
float i_mag = sqrtf(SQ(iq_avg) + SQ(id_avg));
10711071
*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,
10821082
float linkage_sum = 0.0;
10831083
float linkage_samples = 0.0;
10841084
for (int i = 0;i < 20000;i++) {
1085-
float rad_s_now = mcpwm_foc_get_rpm_faster() * RPM2RADPS_f;
1085+
float rad_s_now = RPM2RADPS_f(mcpwm_foc_get_rpm_faster());
10861086
if (fabsf(mcpwm_foc_get_duty_cycle_now()) < 0.01) {
10871087
break;
10881088
}

‎encoder.c

+1-1
Original file line numberDiff line numberDiff line change
@@ -548,7 +548,7 @@ float encoder_read_deg(void) {
548548
UTILS_LP_FAST(sincos_signal_above_max_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
549549
UTILS_LP_FAST(sincos_signal_low_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
550550

551-
float angle_tmp = utils_fast_atan2(sin, cos) * RAD2DEG_f;
551+
float angle_tmp = RAD2DEG_f(utils_fast_atan2(sin, cos));
552552
UTILS_LP_FAST(angle, angle_tmp, sincos_filter_constant);
553553
last_enc_angle = angle;
554554
}

‎imu/imu.c

+17-17
Original file line numberDiff line numberDiff line change
@@ -334,7 +334,7 @@ void imu_get_calibration(float yaw, float *imu_cal) {
334334
roll_sample = roll_sample / 250;
335335

336336
// Set roll rotations to level out roll axis
337-
m_settings.rot_roll = -roll_sample * RAD2DEG_f;
337+
m_settings.rot_roll = -RAD2DEG_f(roll_sample);
338338

339339
// Rotate gyro offsets to match new IMU orientation
340340
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) {
353353
pitch_sample = pitch_sample / 250;
354354

355355
// Set pitch rotation to level out pitch axis
356-
m_settings.rot_pitch = pitch_sample * RAD2DEG_f;
356+
m_settings.rot_pitch = RAD2DEG_f(pitch_sample);
357357

358358
// Rotate imu offsets to match
359359
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) {
445445

446446
// Rotate axes (ZYX)
447447

448-
float s1 = sinf(m_settings.rot_yaw * DEG2RAD_f);
449-
float c1 = cosf(m_settings.rot_yaw * DEG2RAD_f);
450-
float s2 = sinf(m_settings.rot_pitch * DEG2RAD_f);
451-
float c2 = cosf(m_settings.rot_pitch * DEG2RAD_f);
452-
float s3 = sinf(m_settings.rot_roll * DEG2RAD_f);
453-
float c3 = cosf(m_settings.rot_roll * DEG2RAD_f);
448+
float s1 = sinf(DEG2RAD_f(m_settings.rot_yaw));
449+
float c1 = cosf(DEG2RAD_f(m_settings.rot_yaw));
450+
float s2 = sinf(DEG2RAD_f(m_settings.rot_pitch));
451+
float c2 = cosf(DEG2RAD_f(m_settings.rot_pitch));
452+
float s3 = sinf(DEG2RAD_f(m_settings.rot_roll));
453+
float c3 = cosf(DEG2RAD_f(m_settings.rot_roll));
454454

455455
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
456456
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) {
475475
}
476476

477477
float gyro_rad[3];
478-
gyro_rad[0] = m_gyro[0] * DEG2RAD_f;
479-
gyro_rad[1] = m_gyro[1] * DEG2RAD_f;
480-
gyro_rad[2] = m_gyro[2] * DEG2RAD_f;
478+
gyro_rad[0] = DEG2RAD_f(m_gyro[0]);
479+
gyro_rad[1] = DEG2RAD_f(m_gyro[1]);
480+
gyro_rad[2] = DEG2RAD_f(m_gyro[2]);
481481

482482
switch (m_settings.mode) {
483483
case AHRS_MODE_MADGWICK:
@@ -508,12 +508,12 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
508508

509509
void rotate(float *input, float *rotation, float *output) {
510510
// Rotate imu offsets to match
511-
float s1 = sinf(rotation[2] * DEG2RAD_f);
512-
float c1 = cosf(rotation[2] * DEG2RAD_f);
513-
float s2 = sinf(rotation[1] * DEG2RAD_f);
514-
float c2 = cosf(rotation[1] * DEG2RAD_f);
515-
float s3 = sinf(rotation[0] * DEG2RAD_f);
516-
float c3 = cosf(rotation[0] * DEG2RAD_f);
511+
float s1 = sinf(DEG2RAD_f(rotation[2]));
512+
float c1 = cosf(DEG2RAD_f(rotation[2]));
513+
float s2 = sinf(DEG2RAD_f(rotation[1]));
514+
float c2 = cosf(DEG2RAD_f(rotation[1]));
515+
float s3 = sinf(DEG2RAD_f(rotation[0]));
516+
float c3 = cosf(DEG2RAD_f(rotation[0]));
517517

518518
float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2;
519519
float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3;

‎mcpwm.c

+3-3
Original file line numberDiff line numberDiff line change
@@ -727,7 +727,7 @@ float mcpwm_get_switching_frequency_now(void) {
727727
*/
728728
float mcpwm_get_rpm(void) {
729729
if (conf->motor_type == MOTOR_TYPE_DC) {
730-
return m_pll_speed * RADPS2RPM_f;
730+
return RADPS2RPM_f(m_pll_speed);
731731
} else {
732732
return direction ? rpm_now : -rpm_now;
733733
}
@@ -2110,7 +2110,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
21102110
if (encoder_is_configured()) {
21112111
float pos = encoder_read_deg();
21122112
run_pid_control_pos(1.0 / switching_frequency_now, pos);
2113-
pll_run(-pos * DEG2RAD_f, 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
2113+
pll_run(-DEG2RAD_f(pos), 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
21142114
}
21152115

21162116
last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
@@ -2164,7 +2164,7 @@ float mcpwm_get_detect_pos(void) {
21642164
v2 /= amp;
21652165

21662166
float ph[1];
2167-
ph[0] = asinf(v0) * RAD2DEG_f;
2167+
ph[0] = RAD2DEG_f(asinf(v0));
21682168

21692169
float res = ph[0];
21702170
if (v1 < v2) {

0 commit comments

Comments
 (0)