FixedWingModeManager: use Sticks library

src/modules/fw_mode_manager/CMakeLists.txt

@@ -41,7 +41,8 @@ set(POSCONTROL_DEPENDENCIES
 	SlewRate
 	tecs
 	motion_planning
-    performance_model
+    	performance_model
+	Sticks
 )
 
 if(CONFIG_FIGURE_OF_EIGHT)

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -207,23 +207,18 @@ FixedWingModeManager::wind_poll(const hrt_abstime now)
 void
 FixedWingModeManager::manual_control_setpoint_poll()
 {
-	_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+	_sticks.checkAndUpdateStickInputs();
 
-	_manual_control_setpoint_for_height_rate = _manual_control_setpoint.pitch;
+	_manual_control_setpoint_for_height_rate = _sticks.getPitch();
-	_manual_control_setpoint_for_airspeed = _manual_control_setpoint.throttle;
+	_manual_control_setpoint_for_airspeed = _sticks.getThrottleZeroCentered();
 
 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
 		/* Alternate stick allocation (similar concept as for multirotor systems:
 		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
 		 */
-		_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.throttle;
-		_manual_control_setpoint_for_airspeed = _manual_control_setpoint.pitch;
-	}
 
-	// send neutral setpoints if no update for 1 s
+		_manual_control_setpoint_for_height_rate = -_sticks.getThrottleZeroCentered();
-	if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
+		_manual_control_setpoint_for_airspeed = _sticks.getPitch();
-		_manual_control_setpoint_for_height_rate = 0.f;
-		_manual_control_setpoint_for_airspeed = 0.f;
 	}
 }
 
@@ -1156,7 +1151,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 		fixed_wing_runway_control_s fw_runway_control{};
 		fw_runway_control.timestamp = now;
 		fw_runway_control.wheel_steering_enabled = true;
-		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f;
+		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
 		_fixed_wing_runway_control_pub.publish(fw_runway_control);
 
@@ -1329,7 +1324,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 		fixed_wing_runway_control_s fw_runway_control{};
 		fw_runway_control.timestamp = now;
 		fw_runway_control.wheel_steering_enabled = true;
-		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f;
+		fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _sticks.getYaw() : 0.f;
 
 		_fixed_wing_runway_control_pub.publish(fw_runway_control);
 
@@ -1578,7 +1573,7 @@ FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, cons
 	fw_runway_control.timestamp = now;
 	fw_runway_control.wheel_steering_enabled = true;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
-			_manual_control_setpoint.yaw : 0.f;
+			_sticks.getYaw() : 0.f;
 
 	_fixed_wing_runway_control_pub.publish(fw_runway_control);
 
@@ -1743,7 +1738,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 	fw_runway_control.timestamp = now;
 	fw_runway_control.wheel_steering_enabled = true;
 	fw_runway_control.wheel_steering_nudging_rate = _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled ?
-			_manual_control_setpoint.yaw : 0.f;
+			_sticks.getYaw() : 0.f;
 
 	_fixed_wing_runway_control_pub.publish(fw_runway_control);
 
@@ -1795,7 +1790,7 @@ FixedWingModeManager::control_manual_altitude(const float control_interval, cons
 	_ctrl_configuration_handler.setPitchMin(min_pitch);
 	_ctrl_configuration_handler.setThrottleMax(throttle_max);
 
-	const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+	const float roll_body = _sticks.getRoll() * radians(_param_fw_r_lim.get());
 	const DirectionalGuidanceOutput sp = {.course_setpoint = NAN, .lateral_acceleration_feedforward = rollAngleToLateralAccel(roll_body)};
 	fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
 	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
@@ -1831,8 +1826,8 @@ FixedWingModeManager::control_manual_position(const hrt_abstime now, const float
 
 	/* heading control */
 	// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
-	if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
+	if (fabsf(_sticks.getRoll()) < HDG_HOLD_MAN_INPUT_THRESH &&
-	    fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) {
+	    fabsf(_sticks.getYaw()) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 		/* heading / roll is zero, lock onto current heading */
 		if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
@@ -1891,13 +1886,13 @@ FixedWingModeManager::control_manual_position(const hrt_abstime now, const float
 	_ctrl_configuration_handler.setPitchMin(min_pitch);
 	_ctrl_configuration_handler.setThrottleMax(throttle_max);
 
-	if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH ||
+	if (!_yaw_lock_engaged || fabsf(_sticks.getRoll()) >= HDG_HOLD_MAN_INPUT_THRESH ||
-	    fabsf(_manual_control_setpoint.yaw) >= HDG_HOLD_MAN_INPUT_THRESH) {
+	    fabsf(_sticks.getYaw()) >= HDG_HOLD_MAN_INPUT_THRESH) {
 
 		_hdg_hold_enabled = false;
 		_yaw_lock_engaged = false;
 
-		const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+		const float roll_body = _sticks.getRoll() * radians(_param_fw_r_lim.get());
 		fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
 		fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
 		fw_lateral_ctrl_sp.lateral_acceleration = rollAngleToLateralAccel(roll_body);
@@ -2385,14 +2380,14 @@ FixedWingModeManager::initializeAutoLanding(const hrt_abstime &now, const positi
 Vector2f
 FixedWingModeManager::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
 {
-	if (fabsf(_manual_control_setpoint.yaw) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
+	if (fabsf(_sticks.getYaw()) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
 	    && _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled
 	    && !_flare_states.flaring) {
 		// laterally nudge touchdown location with yaw stick
 		// positive is defined in the direction of a right hand turn starting from the approach vector direction
 		const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
-				_manual_control_setpoint.yaw);
+				_sticks.getYaw());
-		_lateral_touchdown_position_offset += (_manual_control_setpoint.yaw - signed_deadzone_threshold) *
+		_lateral_touchdown_position_offset += (_sticks.getYaw() - signed_deadzone_threshold) *
 						      MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
 		_lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
 						     _param_fw_lnd_td_off.get());

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -52,6 +52,7 @@
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
+#include <lib/sticks/Sticks.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -71,7 +72,6 @@
 #include <uORB/topics/fixed_wing_runway_control.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/launch_detection_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/normalized_unsigned_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_landing_status.h>
@@ -180,7 +180,6 @@ private:
 	uORB::Subscription _wind_sub{ORB_ID(wind)};
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
@@ -201,7 +200,6 @@ private:
 	uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
 	uORB::Publication<fixed_wing_runway_control_s> _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)};
 
-	manual_control_setpoint_s _manual_control_setpoint{};
 	position_setpoint_triplet_s _pos_sp_triplet{};
 	vehicle_control_mode_s _control_mode{};
 	vehicle_local_position_s _local_pos{};
@@ -243,6 +241,9 @@ private:
 		STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
 	};
 
+
+	Sticks _sticks{this};
+
 	// VEHICLE STATES
 
 	uint8_t _nav_state;