diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 6a178a5b1c..26912c0a29 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 27e453c15c..a0c46372b0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/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) _param_fw_thr_min, (ParamFloat) _param_fw_thr_slew_max, + (ParamBool) _param_fw_posctl_inv_st, + // external parameters (ParamInt) _param_fw_arsp_mode, 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 ab45c9413f..5df640bf44 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 @@ -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);