From e5bd6e289e902c3c4183839b9a5ebe4b279ec266 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Fri, 10 Nov 2017 14:23:10 -0500
Subject: [PATCH] AP_Motors: Add current limiting to 6DOF motors for Sub

---
 libraries/AP_Motors/AP_Motors6DOF.cpp      | 43 ++++++++++++++++++++++
 libraries/AP_Motors/AP_Motors6DOF.h        |  6 +++
 libraries/AP_Motors/AP_MotorsMulticopter.h |  2 +-
 3 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp
index 4a27fd5a4e..0c278ab412 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.cpp
+++ b/libraries/AP_Motors/AP_Motors6DOF.cpp
@@ -17,6 +17,7 @@
  *       AP_Motors6DOF.cpp - ArduSub motors library
  */
 
+#include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Motors6DOF.h"
 
@@ -266,6 +267,11 @@ void AP_Motors6DOF::output_to_motors()
     }
 }
 
+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
@@ -338,6 +344,43 @@ void AP_Motors6DOF::output_armed_stabilizing()
             }
         }
     }
+
+    const AP_BattMonitor &battery = AP::battery();
+
+	// Current limiting
+    if (_batt_current_max <= 0.0f || !battery.has_current()) {
+        return;
+    }
+
+    float _batt_current = battery.current_amps();
+
+    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.5f) {
+        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
diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h
index a080fc315b..108e1dc896 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.h
+++ b/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() override;
 
     //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);
@@ -60,4 +62,8 @@ protected:
     float               _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
     float               _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
     float               _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to lateral (left/right)
+
+    // current limiting
+    float _output_limited = 1.0f;
+    float _batt_current_last = 0.0f;
 };
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h
index 348583b787..00b11a7d3a 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.h
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.h
@@ -111,7 +111,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;