manual_control: use filter to check user override

Instead of only looking at the diff between two RC samples to decide
whether a user wants to override, we now look at the filtered diff over
one second. This should be more robust to RC sent at various or varying
rates.

src/modules/manual_control/ManualControl.cpp

@@ -89,6 +89,7 @@ void ManualControl::Run()
 
 	bool found_at_least_one = false;
 	const hrt_abstime now = hrt_absolute_time();
+	const float dt_s = now - _last_time;
 
 	for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) {
 		manual_control_input_s manual_control_input;
@@ -143,21 +144,19 @@ void ManualControl::Run()
 		// user wants override
 		const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
 
-		// TODO: look at least at 3 samples in a specific time
-
-		const bool rpy_moved = (fabsf(_selector.setpoint().x - _previous_x) > minimum_stick_change)
-				       || (fabsf(_selector.setpoint().y - _previous_y) > minimum_stick_change)
-				       || (fabsf(_selector.setpoint().r - _previous_r) > minimum_stick_change);
+		const bool rpy_moved = (fabsf(_x_diff.diff()) > minimum_stick_change)
+				       || (fabsf(_y_diff.diff()) > minimum_stick_change)
+				       || (fabsf(_r_diff.diff()) > minimum_stick_change);
 
 		// Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1]
-		const bool throttle_moved = (fabsf(_selector.setpoint().z - _previous_z) * 2.f > minimum_stick_change);
+		const bool throttle_moved = (fabsf(_z_diff.diff()) * 2.f > minimum_stick_change);
 
 		_selector.setpoint().user_override = rpy_moved || throttle_moved;
 
-		_previous_x = _selector.setpoint().x;
-		_previous_y = _selector.setpoint().y;
-		_previous_z = _selector.setpoint().z;
-		_previous_r = _selector.setpoint().r;
+		_x_diff.update(_selector.setpoint().x, dt_s);
+		_y_diff.update(_selector.setpoint().y, dt_s);
+		_z_diff.update(_selector.setpoint().z, dt_s);
+		_r_diff.update(_selector.setpoint().r, dt_s);
 
 		if (switches_updated) {
 			// Only use switches if current source is RC as well.
@@ -268,12 +267,14 @@ void ManualControl::Run()
 			_manual_control_setpoint_pub.publish(_selector.setpoint());
 		}
 
-		_previous_x = NAN;
-		_previous_y = NAN;
-		_previous_z = NAN;
-		_previous_r = NAN;
+		_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);
 	}
 
+	_last_time = now;
+
 	// reschedule timeout
 	ScheduleDelayed(200_ms);
 

src/modules/manual_control/ManualControl.hpp

@@ -54,6 +54,35 @@ using namespace time_literals;
 namespace manual_control
 {
 
+class MovingDiff
+{
+public:
+	void update(float value, float dt_s)
+	{
+		if (dt_s < 0.0f) {
+			dt_s = 0.0f;
+
+		} else if (dt_s > _time_period_s) {
+			dt_s = _time_period_s;
+		}
+
+		const float new_diff = value - _last_value;
+		_diff = new_diff * dt_s + _diff * (_time_period_s - dt_s);
+		_last_value = value;
+	}
+
+	float diff() const
+	{
+		return _diff;
+	}
+
+private:
+	float _diff{0.0f};
+	float _last_value{0.0f};
+	const float _time_period_s{1.0f};
+};
+
+
 class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -114,15 +143,17 @@ private:
 	bool _previous_arm_gesture{false};
 	bool _previous_disarm_gesture{false};
 
-	float _previous_x{NAN};
-	float _previous_y{NAN};
-	float _previous_z{NAN};
-	float _previous_r{NAN};
+	MovingDiff _x_diff{};
+	MovingDiff _y_diff{};
+	MovingDiff _z_diff{};
+	MovingDiff _r_diff{};
 
 	manual_control_switches_s _previous_switches{};
 	bool _previous_switches_initialized{false};
 	int32_t _last_mode_slot_flt{-1};
 
+	hrt_abstime _last_time{0};
+
 	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};