add voltage magnitude clamping for gimbal motors in closed loop

CHANGELOG.md

@@ -4,6 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
 ### Added
 * `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
 * `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward.
+* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
 
 # Releases
 ## [0.4.7] - 2018-11-28

Firmware/MotorControl/controller.cpp

@@ -183,8 +183,13 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
     Iq += vel_integrator_current_;
 
     // Current limiting
-    float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
     bool limited = false;
+    float Ilim = 0.0f;
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
+        Ilim = std::min(axis_->motor_.config_.current_lim, 0.98f*one_by_sqrt3*vbus_voltage);
+    } else {
+        Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
+    }
     if (Iq > Ilim) {
         limited = true;
         Iq = Ilim;

Firmware/MotorControl/motor.cpp

@@ -282,7 +282,6 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
     return true;
 }
 
-// TODO: This doesn't update brake current
 // We should probably make FOC Current call FOC Voltage to avoid duplication.
 bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
     float c = our_arm_cos_f32(phase);