manual_control: move override detection

This also removes the option to ignore throttle for the override
detection as it's not really required anymore.

msg/manual_control_setpoint.msg

@@ -43,12 +43,6 @@ float32 r			 # yaw stick/twist position, -1..1
 				 # in general corresponds to the righthand rotation around the vertical
 				 # (downwards) axis of the vehicle
 
-# stick velocity
-float32 vx
-float32 vy
-float32 vz
-float32 vr
-
 float32 flaps			 # flap position
 
 float32 aux1

src/modules/commander/ManualControl.cpp

@@ -36,10 +36,9 @@
 
 using namespace time_literals;
 
-enum OverrideBits {
+enum class OverrideBits : int32_t {
 	OVERRIDE_AUTO_MODE_BIT = (1 << 0),
 	OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
-	OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
 };
 
 bool ManualControl::update()
@@ -63,10 +62,10 @@ bool ManualControl::update()
 bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode,
 				  const vehicle_status_s &vehicle_status)
 {
-	const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
+	const bool override_auto_mode = (_param_rc_override.get() & static_cast<int32_t>(OverrideBits::OVERRIDE_AUTO_MODE_BIT))
 					&& vehicle_control_mode.flag_control_auto_enabled;
 
-	const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
+	const bool override_offboard_mode = (_param_rc_override.get() & static_cast<int32_t>(OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT))
 					    && vehicle_control_mode.flag_control_offboard_enabled;
 
 	// in Descend manual override is enbaled independently of COM_RC_OVERRIDE
@@ -74,19 +73,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
 
 
 	if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
-		const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
-
-		const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)
-				       || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change)
-				       || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > 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(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
-		const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
-
-		if (rpy_moved || (use_throttle && throttle_moved)) {
-			return true;
-		}
+
+		return _manual_control_setpoint.user_override;
 	}
 
 	return false;

src/modules/commander/commander_params.c

@@ -669,10 +669,9 @@ PARAM_DEFINE_INT32(COM_REARM_GRACE, 1);
  * override is always enabled.
  *
  * @min 0
- * @max 7
+ * @max 3
  * @bit 0 Enable override during auto modes (except for in critical battery reaction)
  * @bit 1 Enable override during offboard mode
- * @bit 2 Ignore throttle stick
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);

src/modules/manual_control/ManualControl.cpp

@@ -137,20 +137,23 @@ void ManualControl::Run()
 		}
 
 		// user wants override
-		//const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
+		const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
 
-		// FIXME: we need previous
-		//const bool rpy_moved = (fabsf(_selector.setpoint().x - _manual_control_input[i].x) > minimum_stick_change)
-		//		       || (fabsf(_selector.setpoint().y - _manual_control_input[i].y) > minimum_stick_change)
-		//		       || (fabsf(_selector.setpoint().r - _manual_control_input[i].r) > minimum_stick_change);
+		// 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);
 
 		// 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 - _manual_control_input[i].z) * 2.f > minimum_stick_change);
-		//const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
+		const bool throttle_moved = (fabsf(_selector.setpoint().z - _previous_z) * 2.f > minimum_stick_change);
 
-		//_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved);
+		_selector.setpoint().user_override = rpy_moved || throttle_moved;
 
-		// TODO: at least 3 samples in a time
+		_previous_x = _selector.setpoint().x;
+		_previous_y = _selector.setpoint().y;
+		_previous_z = _selector.setpoint().z;
+		_previous_r = _selector.setpoint().r;
 
 		if (switches_updated) {
 			// Only use switches if current source is RC as well.
@@ -177,6 +180,11 @@ void ManualControl::Run()
 			_published_invalid_once = true;
 			_manual_control_setpoint_pub.publish(_selector.setpoint());
 		}
+
+		_previous_x = NAN;
+		_previous_y = NAN;
+		_previous_z = NAN;
+		_previous_r = NAN;
 	}
 
 	// reschedule timeout

src/modules/manual_control/ManualControl.hpp

@@ -76,13 +76,6 @@ public:
 private:
 	static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
 
-
-	enum OverrideBits {
-		OVERRIDE_AUTO_MODE_BIT       = (1 << 0),
-		OVERRIDE_OFFBOARD_MODE_BIT   = (1 << 1),
-		OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
-	};
-
 	void Run() override;
 
 	void send_arm_command();
@@ -108,13 +101,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};
+
 	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")};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
 		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
-		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
 		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
 		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst
 	)