FW Position controller: add option to swap throttle and pitch stick

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -202,6 +202,24 @@ FixedwingPositionControl::airspeed_poll()
 	}
 }
 
+void
+FixedwingPositionControl::manual_control_setpoint_poll()
+{
+	_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+
+	_manual_control_setpoint_altitude = _manual_control_setpoint.x;
+	_manual_control_setpoint_airspeed = _manual_control_setpoint.z;
+
+	if (_param_fw_posctl_inv_st.get()) {
+		/* 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_altitude = -(_manual_control_setpoint.z * 2.f - 1.f);
+		_manual_control_setpoint_airspeed = _manual_control_setpoint.x / 2.f + 0.5f;
+	}
+}
+
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -229,17 +247,17 @@ FixedwingPositionControl::get_demanded_airspeed()
 	float altctrl_airspeed = 0;
 
 	// neutral throttle corresponds to trim airspeed
-	if (_manual_control_setpoint.z < 0.5f) {
+	if (_manual_control_setpoint_airspeed < 0.5f) {
 		// lower half of throttle is min to trim airspeed
 		altctrl_airspeed = _param_fw_airspd_min.get() +
 				   (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
-				   _manual_control_setpoint.z * 2;
+				   _manual_control_setpoint_airspeed * 2;
 
 	} else {
 		// upper half of throttle is trim to max airspeed
 		altctrl_airspeed = _param_fw_airspd_trim.get() +
 				   (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
-				   (_manual_control_setpoint.z * 2 - 1);
+				   (_manual_control_setpoint_airspeed * 2 - 1);
 	}
 
 	return altctrl_airspeed;
@@ -483,15 +501,15 @@ FixedwingPositionControl::update_desired_altitude(float dt)
 	 * an axis. Positive X means to rotate positively around
 	 * the X axis in NED frame, which is pitching down
 	 */
-	if (_manual_control_setpoint.x > deadBand) {
+	if (_manual_control_setpoint_altitude > deadBand) {
 		/* pitching down */
-		float pitch = -(_manual_control_setpoint.x - deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
 		_hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch;
 		_was_in_deadband = false;
 
-	} else if (_manual_control_setpoint.x < - deadBand) {
+	} else if (_manual_control_setpoint_altitude < - deadBand) {
 		/* pitching up */
-		float pitch = -(_manual_control_setpoint.x + deadBand) / factor;
+		float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
 		_hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch;
 		_was_in_deadband = false;
 		climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
@@ -927,7 +945,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		/* throttle limiting */
 		throttle_max = _param_fw_thr_max.get();
 
-		if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
+		if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
 			throttle_max = 0.0f;
 		}
 
@@ -1029,7 +1047,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		/* throttle limiting */
 		throttle_max = _param_fw_thr_max.get();
 
-		if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint.z) < THROTTLE_THRESH)) {
+		if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
 			throttle_max = 0.0f;
 		}
 
@@ -1659,14 +1677,13 @@ FixedwingPositionControl::Run()
 		_alt_reset_counter = _local_pos.vz_reset_counter;
 		_pos_reset_counter = _local_pos.vxy_reset_counter;
 
-		airspeed_poll();
-		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
-
 		if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
 			// reset the altitude foh (first order hold) logic
 			_min_current_sp_distance_xy = FLT_MAX;
 		}
 
+		manual_control_setpoint_poll();
+		_pos_sp_triplet_sub.update(&_pos_sp_triplet);
 		vehicle_attitude_poll();
 		vehicle_command_poll();
 		vehicle_control_mode_poll();

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -261,6 +261,9 @@ private:
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position
 	uint8_t _alt_reset_counter{0};				///< captures the number of times the estimator has reset the altitude state
 
+	float _manual_control_setpoint_altitude{0.0f};
+	float _manual_control_setpoint_airspeed{0.0f};
+
 	ECL_L1_Pos_Controller	_l1_control;
 	TECS			_tecs;
 
@@ -281,6 +284,7 @@ private:
 	// Update subscriptions
 	void		airspeed_poll();
 	void		control_update();
+	void 		manual_control_setpoint_poll();
 	void		vehicle_attitude_poll();
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
@@ -415,6 +419,8 @@ private:
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
 		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
 
+		(ParamBool<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
+
 		// external parameters
 		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -724,3 +724,16 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
+
+/**
+ * RC stick mapping fixed-wing.
+ *
+ * Set RC/joystick configuration for fixed-wing position and altitude controlled flight.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Normal stick configuration (airspeed on throttle stick, altitude on pitch stick)
+ * @value 1 Alternative stick configuration (altitude on throttle stick, airspeed on pitch stick)
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0);