From 632dfa55e6f231f1b62fb2e9bfb048d391ebafc9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 12 Jan 2022 17:33:20 +0100 Subject: [PATCH] FW Pos C: add option to disable airspeed setpoint via stick input -rename FW_POSCTL_INV_ST to FW_POS_STK_CONF and make bitmask out of it: - bit 0: alternative mapping (height rate on throttle stick, airspeed on pitch) - bit 1: enable/disable airspeed setpoints via stick Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 34 ++++++++++++------- .../FixedwingPositionControl.hpp | 7 +++- .../fw_pos_control_l1_params.c | 12 +++---- 3 files changed, 33 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3205fa96e1..6b79350f98 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -325,13 +325,19 @@ FixedwingPositionControl::manual_control_setpoint_poll() _manual_control_setpoint_altitude = _manual_control_setpoint.x; _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); - if (_param_fw_posctl_inv_st.get()) { + 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_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; } + + // send neutral setpoints if no update for 1 s + if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) { + _manual_control_setpoint_altitude = 0.f; + _manual_control_setpoint_airspeed = 0.5f; + } } @@ -375,20 +381,22 @@ FixedwingPositionControl::vehicle_attitude_poll() float FixedwingPositionControl::get_manual_airspeed_setpoint() { - float altctrl_airspeed = 0; + float altctrl_airspeed = _param_fw_airspd_trim.get(); - // neutral throttle corresponds to trim airspeed - 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_airspeed * 2; + if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { + // neutral throttle corresponds to trim airspeed + 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_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_airspeed * 2 - 1); + } 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_airspeed * 2 - 1); + } } return altctrl_airspeed; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 2a803cc7fc..1a79dfd554 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -286,6 +286,11 @@ private: param_t _param_handle_airspeed_trans{PARAM_INVALID}; float _param_airspeed_trans{NAN}; + enum StickConfig { + STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0), + STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1) + }; + // Update our local parameter cache. int parameters_update(); @@ -461,7 +466,7 @@ private: (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_slew_max, - (ParamBool) _param_fw_posctl_inv_st, + (ParamInt) _param_fw_pos_stk_conf, (ParamInt) _param_nav_gpsf_lt, (ParamFloat) _param_nav_gpsf_r, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index b972293ab6..76ec5ce013 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -885,17 +885,17 @@ PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f); PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); /** - * RC stick mapping fixed-wing. + * RC stick configuraton fixed-wing. * - * Set RC/joystick configuration for fixed-wing position and altitude controlled flight. + * Set RC/joystick configuration for fixed-wing manual 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) + * @max 3 + * @bit 0 Alternative stick configuration (height rate on throttle stick, airspeed on pitch stick) + * @bit 1 Enable airspeed setpoint via sticks in altitude and position flight mode * @group FW L1 Control */ -PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0); +PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2); /** * Specific total energy rate first order filter time constant.