From d0fea93d72517fc5a2f0320d61f4e326b333c442 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 10 Jun 2021 11:48:17 +0200
Subject: [PATCH] manual_control: add reset to diff

This should prevent triggering user override right after RC re-appears.
---
 src/modules/manual_control/ManualControl.cpp |  8 ++++----
 src/modules/manual_control/ManualControl.hpp | 19 +++++++++++++++----
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp
index 0c58a5c564..5afd897626 100644
--- a/src/modules/manual_control/ManualControl.cpp
+++ b/src/modules/manual_control/ManualControl.cpp
@@ -275,10 +275,10 @@ void ManualControl::Run()
 			_manual_control_setpoint_pub.publish(_selector.setpoint());
 		}
 
-		_x_diff.update(0.0f, dt_s);
-		_y_diff.update(0.0f, dt_s);
-		_z_diff.update(0.0f, dt_s);
-		_r_diff.update(0.0f, dt_s);
+		_x_diff.reset();
+		_y_diff.reset();
+		_z_diff.reset();
+		_r_diff.reset();
 	}
 
 	_last_time = now;
diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp
index 604410d827..2cfc9db9a2 100644
--- a/src/modules/manual_control/ManualControl.hpp
+++ b/src/modules/manual_control/ManualControl.hpp
@@ -66,8 +66,12 @@ public:
 			dt_s = _time_period_s;
 		}
 
-		const float new_diff = value - _last_value;
-		_diff = new_diff * dt_s + _diff * (_time_period_s - dt_s);
+		// Leave _diff at 0.0f if we don't have a _last_value yet.
+		if (PX4_ISFINITE(_last_value)) {
+			const float new_diff = value - _last_value;
+			_diff = new_diff * dt_s + _diff * (_time_period_s - dt_s);
+		}
+
 		_last_value = value;
 	}
 
@@ -76,10 +80,17 @@ public:
 		return _diff;
 	}
 
+	void reset()
+	{
+		_diff = 0.0f;
+		_last_value = NAN;
+	}
+
 private:
+	static constexpr float _time_period_s{1.0f};
+
 	float _diff{0.0f};
-	float _last_value{0.0f};
-	const float _time_period_s{1.0f};
+	float _last_value{NAN};
 };