Commander: split out rc override logic into ManualControl

src/modules/commander/CMakeLists.txt

@@ -50,6 +50,7 @@ px4_add_module(
 		level_calibration.cpp
 		lm_fit.cpp
 		mag_calibration.cpp
+		ManualControl.cpp
 		rc_calibration.cpp
 		state_machine_helper.cpp
 		worker_thread.cpp

src/modules/commander/Commander.cpp

@@ -1991,8 +1991,8 @@ Commander::run()
 			}
 		}
 
-		// update manual_control_setpoint before geofence (which might check sticks or switches)
+		_manual_control.update();
-		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+		_manual_control_setpoint = _manual_control._manual_control_setpoint;
 
 		/* start geofence result check */
 		_geofence_result_sub.update(&_geofence_result);
@@ -2096,33 +2096,16 @@ Commander::run()
 		}
 
 		// abort autonomous mode and switch to position mode if sticks are moved significantly
-		if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
+		if ((_param_rc_override.get() != 0)
-
+		    && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
-			const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT)
+		    && !in_low_battery_failsafe && !_geofence_warning_action_on
-							&& _vehicle_control_mode.flag_control_auto_enabled;
+		    && _manual_control.wantsOverride(_param_rc_override.get(), _param_com_rc_stick_ov.get(), _vehicle_control_mode,
-
+						     !_status.rc_signal_lost)) {
-			const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT)
+			if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
-							    && _vehicle_control_mode.flag_control_offboard_enabled;
+						  &_internal_state) == TRANSITION_CHANGED) {
-
+				tune_positive(true);
-			if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) {
+				mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
-				const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get();
+				_status_changed = true;
-
-				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);
-				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() & OVERRIDE_IGNORE_THROTTLE_BIT);
-
-				if (!_status.rc_signal_lost &&
-				(rpy_moved || (use_throttle && throttle_moved))) {
-					if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
-								  &_internal_state) == TRANSITION_CHANGED) {
-						tune_positive(true);
-						mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
-						_status_changed = true;
-					}
-				}
 			}
 		}
 

src/modules/commander/Commander.hpp

@@ -37,6 +37,7 @@
 /*   Helper classes  */
 #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
+#include "ManualControl.hpp"
 #include "state_machine_helper.h"
 #include "worker_thread.hpp"
 
@@ -70,7 +71,6 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
@@ -286,12 +286,6 @@ private:
 		ALWAYS = 2
 	};
 
-	enum OverrideMode {
-		OVERRIDE_AUTO_MODE_BIT = (1 << 0),
-		OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
-		OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
-	};
-
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 	static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)};
@@ -367,6 +361,7 @@ private:
 	manual_control_setpoint_s _last_manual_control_setpoint{};
 	manual_control_switches_s _manual_control_switches{};
 	manual_control_switches_s _last_manual_control_switches{};
+	ManualControl _manual_control;
 	hrt_abstime	_rc_signal_lost_timestamp{0};		///< Time at which the RC reception was lost
 	int32_t		_flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
 	uint8_t		_last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
@@ -417,7 +412,6 @@ private:
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_safety_sub{ORB_ID(safety)};
-	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_manual_control_switches_sub{ORB_ID(manual_control_switches)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};

src/modules/commander/ManualControl.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "ManualControl.hpp"
+
+enum OverrideBits {
+	OVERRIDE_AUTO_MODE_BIT = (1 << 0),
+	OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1),
+	OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2)
+};
+
+void ManualControl::update()
+{
+	if (_manual_control_setpoint_sub.updated()) {
+		manual_control_setpoint_s manual_control_setpoint;
+
+		if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+			process(manual_control_setpoint);
+		}
+	}
+}
+
+void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint)
+{
+	_last_manual_control_setpoint = _manual_control_setpoint;
+	_manual_control_setpoint = manual_control_setpoint;
+}
+
+bool ManualControl::wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov,
+				  const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available)
+{
+	const bool override_auto_mode = (param_rc_override & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
+					&& vehicle_control_mode.flag_control_auto_enabled;
+
+	const bool override_offboard_mode = (param_rc_override & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
+					    && vehicle_control_mode.flag_control_offboard_enabled;
+
+	if (rc_available && (override_auto_mode || override_offboard_mode)) {
+		const float minimum_stick_change = .01f * param_com_rc_stick_ov;
+
+		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);
+		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 & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT);
+
+		if (rpy_moved || (use_throttle && throttle_moved)) {
+			return true;
+		}
+	}
+
+	return false;
+}

src/modules/commander/ManualControl.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ManualControl.hpp
+ *
+ * @brief Logic for handling RC or Joystick input
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
+class ManualControl
+{
+public:
+	ManualControl() = default;
+	~ManualControl() = default;
+
+	void update();
+	bool wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov,
+			   const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available);
+
+//private:
+	void process(manual_control_setpoint_s &manual_control_setpoint);
+
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	manual_control_setpoint_s _manual_control_setpoint{};
+	manual_control_setpoint_s _last_manual_control_setpoint{};
+};