From a5783939ef661620b5f1c1dc21dbda573c49ae6d Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 9 Aug 2017 16:20:44 +0900
Subject: [PATCH] AP_MotorsUGV: add limit flags

These flags become true when the steering servo or motors hit their
limits.  used to stop I-term build-up in higher level controllers.
---
 APMrover2/AP_MotorsUGV.cpp | 28 ++++++++++++++++++++++++++++
 APMrover2/AP_MotorsUGV.h   | 11 +++++++++++
 2 files changed, 39 insertions(+)

diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp
index b839d2a548..8f73eeea76 100644
--- a/APMrover2/AP_MotorsUGV.cpp
+++ b/APMrover2/AP_MotorsUGV.cpp
@@ -100,6 +100,9 @@ void AP_MotorsUGV::output(bool armed, float dt)
 
     slew_limit_throttle(dt);
 
+    // clear and set limits based on input (limit flags may be set again by output_regular or output_skid_steering methods)
+    set_limits_from_input(armed, _steering, _throttle);
+
     // output for regular steering/throttle style frames
     output_regular(armed, _steering, _throttle);
 
@@ -137,6 +140,10 @@ void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
 // output to skid steering channels
 void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
 {
+    if (!have_skid_steering()) {
+        return;
+    }
+
     // handle simpler disarmed case
     if (!armed) {
         if (_disarm_disable_pwm) {
@@ -162,6 +169,17 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott
     if (saturation_value > 1.0f) {
         steering_scaled = steering_scaled / saturation_value;
         throttle_scaled = throttle_scaled / saturation_value;
+        // set limits
+        if (is_negative(steering)) {
+            limit.steer_left = true;
+        } else {
+            limit.steer_right = true;
+        }
+        if (is_negative(throttle)) {
+            limit.throttle_lower = true;
+        } else {
+            limit.throttle_upper = true;
+        }
     }
 
     // add in throttle
@@ -245,6 +263,16 @@ void AP_MotorsUGV::slew_limit_throttle(float dt)
     }
 }
 
+// set limits based on steering and throttle input
+void AP_MotorsUGV::set_limits_from_input(bool armed, float steering, float throttle)
+{
+    // set limits based on inputs
+    limit.steer_left = !armed || (steering <= -4500.0f);
+    limit.steer_right = !armed || (steering >= 4500.0f);
+    limit.throttle_lower = !armed || (throttle <= -100.0f);
+    limit.throttle_upper = !armed || (throttle >= 100.0f);
+}
+
 // setup servo output
 void AP_MotorsUGV::setup_servo_output()
 {
diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h
index 42873f174e..8e34db5792 100644
--- a/APMrover2/AP_MotorsUGV.h
+++ b/APMrover2/AP_MotorsUGV.h
@@ -57,6 +57,14 @@ public:
     // test steering or throttle output using a pwm value
     bool output_test_pwm(motor_test_order motor_seq, float pwm);
 
+    // structure for holding motor limit flags
+    struct AP_MotorsUGV_limit {
+        uint8_t steer_left      : 1; // we have reached the steering controller's left most limit
+        uint8_t steer_right     : 1; // we have reached the steering controller's right most limit
+        uint8_t throttle_lower  : 1; // we have reached throttle's lower limit
+        uint8_t throttle_upper  : 1; // we have reached throttle's upper limit
+    } limit;
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -77,6 +85,9 @@ protected:
     // slew limit throttle for one iteration
     void slew_limit_throttle(float dt);
 
+    // set limits based on steering and throttle input
+    void set_limits_from_input(bool armed, float steering, float throttle);
+
     // external references
     AP_ServoRelayEvents &_relayEvents;