From 18d93a72ef6a096e9f59e90a22ac39eb7f8854f1 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Tue, 24 Apr 2018 18:34:04 -0400
Subject: [PATCH] Sub: Limit input demand rate of change on motor mixer in
 manual mode

---
 ArduSub/Sub.h              | 2 ++
 ArduSub/control_manual.cpp | 3 +++
 ArduSub/flight_mode.cpp    | 2 ++
 3 files changed, 7 insertions(+)

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 9b74f750aa..4c2d53cad1 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -320,6 +320,8 @@ private:
 
     AP_Arming_Sub arming {ahrs, barometer, compass, battery};
 
+    uint32_t last_control_mode_update_us; // dt tracking for control mode updates
+
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;
diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp
index 3671ea5fe4..2b12cf6b12 100644
--- a/ArduSub/control_manual.cpp
+++ b/ArduSub/control_manual.cpp
@@ -32,4 +32,7 @@ void Sub::manual_run()
     motors.set_throttle(channel_throttle->norm_input());
     motors.set_forward(channel_forward->norm_input());
     motors.set_lateral(channel_lateral->norm_input());
+
+    float dt =  ((float)(micros() - last_control_mode_update_us)) / 1e6;
+    motors.limit_demand_slew_rate(g.manual_slew_rate, dt);
 }
diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp
index 9070a5ac71..594c072376 100644
--- a/ArduSub/flight_mode.cpp
+++ b/ArduSub/flight_mode.cpp
@@ -139,6 +139,8 @@ void Sub::update_flight_mode()
     default:
         break;
     }
+
+    last_control_mode_update_us = micros();
 }
 
 // exit_mode - high level call to organise cleanup as a flight mode is exited