fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 00:50:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+21 -13
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;