add ACIM autoflux

Firmware/MotorControl/controller.cpp

@@ -184,6 +184,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
             effective_flux = std::copysignf(minflux, effective_flux);
         vel_gain /= effective_flux;
         vel_integrator_gain /= effective_flux;
+        // TODO: also scale the integral value which is also changing units.
+        // (or again just do control in torque units)
     }
 
     // Velocity control

Firmware/MotorControl/motor.cpp

@@ -457,6 +457,14 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
         // However the rotor time constant is (usually) so slow that it doesn't matter
         // 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 gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
+            id += gain * (abs_iq - id) * current_meas_period;
+            id = MACRO_MIN(MACRO_MAX(id, config_.acim_autoflux_min_Id), ilim);
+            current_control_.Id_setpoint = id;
+        }
+
         // acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
         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;

Firmware/MotorControl/motor.hpp

@@ -82,6 +82,10 @@ public:
         float inverter_temp_limit_upper = 120;
         float acim_slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
         float acim_gain_min_flux = 10; // [A]
+        float acim_autoflux_min_Id = 10; // [A]
+        bool acim_autoflux_enable = false;
+        float acim_autoflux_attack_gain = 10.0f;
+        float acim_autoflux_decay_gain = 1.0f;
     };
 
     enum TimingLog_t {
@@ -251,7 +255,11 @@ public:
                 make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth,
                     [](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this),
                 make_protocol_property("acim_slip_velocity", &config_.acim_slip_velocity),
-                make_protocol_property("acim_gain_min_flux", &config_.acim_gain_min_flux)
+                make_protocol_property("acim_gain_min_flux", &config_.acim_gain_min_flux),
+                make_protocol_property("acim_autoflux_min_Id", &config_.acim_autoflux_min_Id),
+                make_protocol_property("acim_autoflux_enable", &config_.acim_autoflux_enable),
+                make_protocol_property("acim_autoflux_attack_gain", &config_.acim_autoflux_attack_gain),
+                make_protocol_property("acim_autoflux_decay_gain", &config_.acim_autoflux_decay_gain)
             )
         );
     }