ManualControl: fix RC override

src/modules/manual_control/ManualControl.cpp

@@ -34,9 +34,6 @@
 #include "ManualControl.hpp"
 #include <uORB/topics/commander_state.h>
 
-namespace manual_control
-{
-
 ManualControl::ManualControl() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -103,19 +100,15 @@ void ManualControl::Run()
 
 		// User override by stick
 		const float dt_s = (now - _last_time) / 1e6f;
-		_x_diff.update(_selector.setpoint().chosen_input.x, dt_s);
-		_y_diff.update(_selector.setpoint().chosen_input.y, dt_s);
-		_z_diff.update(_selector.setpoint().chosen_input.z, dt_s);
-		_r_diff.update(_selector.setpoint().chosen_input.r, dt_s);
-
 		const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
 
-		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);
+		const bool rpy_moved = (fabsf(_x_diff.update(_selector.setpoint().chosen_input.x, dt_s)) > minimum_stick_change)
+				       || (fabsf(_y_diff.update(_selector.setpoint().chosen_input.y, dt_s)) > minimum_stick_change)
+				       || (fabsf(_r_diff.update(_selector.setpoint().chosen_input.r, dt_s)) > 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(_z_diff.diff()) * 2.f) > minimum_stick_change;
+		const bool throttle_moved =
+			(fabsf(_z_diff.update(_selector.setpoint().chosen_input.z, dt_s)) * 2.f) > minimum_stick_change;
 
 		_selector.setpoint().user_override = rpy_moved || throttle_moved;
 
@@ -398,9 +391,7 @@ Module consuming manual_control_inputs publishing one manual_control_setpoint.
 	return 0;
 }
 
-}; // namespace manual_control
-
 extern "C" __EXPORT int manual_control_main(int argc, char *argv[])
 {
-	return manual_control::ManualControl::main(argc, argv);
+	return ManualControl::main(argc, argv);
 }

src/modules/manual_control/ManualControl.hpp

@@ -51,52 +51,10 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include "ManualControlSelector.hpp"
+#include "MovingDiff.hpp"
 
 using namespace time_literals;
 
-namespace manual_control
-{
-
-class MovingDiff
-{
-public:
-	void update(float value, float dt_s)
-	{
-		if (!PX4_ISFINITE(value)) {
-			// Ignore NAN
-			return;
-		}
-
-		math::constrain(dt_s, 0.f, _time_period_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;
-	}
-
-	float diff() const
-	{
-		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{NAN};
-};
-
-
 class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -173,4 +131,3 @@ class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, pub
 		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
 	)
 };
-} // namespace manual_control

src/modules/manual_control/ManualControlSelector.cpp

@@ -33,10 +33,6 @@
 
 #include "ManualControlSelector.hpp"
 
-
-namespace manual_control
-{
-
 void ManualControlSelector::updateValidityOfChosenInput(uint64_t now)
 {
 	if (!isInputValid(_setpoint.chosen_input, now)) {
@@ -86,5 +82,3 @@ manual_control_setpoint_s &ManualControlSelector::setpoint()
 {
 	return _setpoint;
 }
-
-} // namespace manual_control

src/modules/manual_control/ManualControlSelector.hpp

@@ -37,9 +37,6 @@
 #include <uORB/topics/manual_control_input.h>
 #include <uORB/topics/manual_control_setpoint.h>
 
-namespace manual_control
-{
-
 class ManualControlSelector
 {
 public:
@@ -58,5 +55,3 @@ class ManualControlSelector
 	int32_t _rc_in_mode{0};
 	int _instance{-1};
 };
-
-} // namespace manual_control

src/modules/manual_control/ManualControlSelectorTest.cpp

@@ -36,7 +36,6 @@
 #include <drivers/drv_hrt.h>
 
 using namespace time_literals;
-using namespace manual_control;
 
 static constexpr uint64_t some_time = 12345678;
 

src/modules/manual_control/MovingDiff.hpp

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+
+class MovingDiff
+{
+public:
+	float update(float value, float dt_s)
+	{
+		if (!PX4_ISFINITE(value) || (dt_s < FLT_EPSILON)) {
+			// Ignore NAN
+			return NAN;
+		}
+
+		_difference_filter.setParameters(dt_s, .1f);
+
+		// 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) / dt_s;
+			_difference_filter.update(new_diff);
+		}
+
+		_last_value = value;
+		return _difference_filter.getState();
+	}
+
+	void reset()
+	{
+		_difference_filter.reset(0.f);
+		_last_value = NAN;
+	}
+
+private:
+	AlphaFilter<float> _difference_filter;
+	float _last_value{NAN};
+};