AP_Motors: Add current limiting to 6DOF motors for Sub

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -242,6 +242,11 @@ void AP_Motors6DOF::output_to_motors()
     hal.rcout->push();
 }
 
+float AP_Motors6DOF::get_current_limit_max_throttle()
+{
+    return 1.0f;
+}
+
 // output_armed - sends commands to the motors
 // includes new scaling stability patch
 // TODO pull code that is common to output_armed_not_stabilizing into helper functions
@@ -314,6 +319,42 @@ void AP_Motors6DOF::output_armed_stabilizing()
             }
         }
     }
+
+	// Current limiting
+    if (_batt_current_max <= 0.0f) {
+        return;
+    }
+
+    static float _output_limited = 1.0f;
+    static float _batt_current_last = 0.0f;
+
+    float _batt_current_delta = _batt_current - _batt_current_last;
+
+    float loop_interval = 1.0f/_loop_rate;
+
+    float _current_change_rate = _batt_current_delta / loop_interval;
+
+    float predicted_current = _batt_current + (_current_change_rate * loop_interval * 5);
+
+    float batt_current_ratio = _batt_current/_batt_current_max;
+
+    float predicted_current_ratio = predicted_current/_batt_current_max;
+    _batt_current_last = _batt_current;
+
+    if (predicted_current > _batt_current_max * 1.5) {
+        batt_current_ratio = 2.5f;
+    } else if (_batt_current < _batt_current_max && predicted_current > _batt_current_max) {
+        batt_current_ratio = predicted_current_ratio;
+    }
+    _output_limited += (loop_interval/(loop_interval+_batt_current_time_constant)) * (1 - batt_current_ratio);
+
+    _output_limited = constrain_float(_output_limited, 0.0f, 1.0f);
+
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            _thrust_rpyt_out[i] *= _output_limited;
+        }
+    }
 }
 
 // output_armed - sends commands to the motors

libraries/AP_Motors/AP_Motors6DOF.h

@@ -45,6 +45,8 @@ public:
     static const struct AP_Param::GroupInfo        var_info[];
 
 protected:
+    // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
+    float               get_current_limit_max_throttle() final;
 
     //Override MotorsMatrix method
     void add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float lat_fac, uint8_t testing_order);

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -114,7 +114,7 @@ protected:
     virtual void        update_throttle_filter();
 
     // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
-    float               get_current_limit_max_throttle();
+    virtual float       get_current_limit_max_throttle();
 
     // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
     float               apply_thrust_curve_and_volt_scaling(float thrust) const;