Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-25 11:06:48 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'madcowswe/clean-math-funcs' into devel

Browse Source
This commit is contained in:
Paul Guenette
2020-10-20 23:32:54 -04:00
parent 44218088c8 074b017ab2
commit b13fbe4163
15 changed files with 192 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/motor.cpp
+8 -9
View File
@@ -261,10 +261,10 @@ bool Motor::run_calibration() {
}
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
if (std::isnan(mod_alpha) || std::isnan(mod_alpha))
if (is_nan(mod_alpha) || is_nan(mod_beta))
return set_error(ERROR_MODULATION_IS_NAN), false;
float tA, tB, tC;
if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
auto [tA, tB, tC, success] = SVM(mod_alpha, mod_beta);
if(!success)
return set_error(ERROR_MODULATION_MAGNITUDE), false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
@@ -350,7 +350,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
// Vector modulation saturation, lock integrator if saturated
// TODO make maximum modulation configurable
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / std::sqrt(mod_d * mod_d + mod_q * mod_q);
if (mod_scalefactor < 1.0f) {
mod_d *= mod_scalefactor;
mod_q *= mod_scalefactor;
@@ -417,7 +417,7 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
phase_vel *= config_.direction;
if (config_.motor_type == MOTOR_TYPE_ACIM) {
current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
current_setpoint = torque_setpoint / (config_.torque_constant * std::max(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
}
else {
current_setpoint = torque_setpoint / config_.torque_constant;
@@ -435,7 +435,7 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
// So we elect to write it as if the effect is immediate, to have cleaner code
if (config_.acim_autoflux_enable) {
float abs_iq = fabsf(iq);
float abs_iq = std::abs(iq);
float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
id += gain * (abs_iq - id) * current_meas_period;
id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
@@ -446,9 +446,8 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux);
current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
// Check for issues with small denominator. Polarity of check to catch NaN too
bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz;
if (!acceptable_vel)
// Check for issues with small denominator.
if (is_nan(slip_velocity) || std::abs(slip_velocity) > 0.1f * (float)current_meas_hz)
slip_velocity = 0.0f;
phase_vel += slip_velocity;
// reporting only: