fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 12:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Pos C: add option to disable airspeed setpoint via stick input

Browse Source 
-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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-01-12 17:33:20 +01:00
parent cbb8c90245
commit 632dfa55e6
3 changed files with 33 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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;

7
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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<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,
(ParamInt<px4::params::FW_POS_STK_CONF>) _param_fw_pos_stk_conf,
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,

12
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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.