fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 13:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Switch manual_control_setpoint.z scaling from [0,1] to [-1,1]

Browse Source 
To be consistent with all other axes of stick input and avoid future
rescaling confusion.

Note: for the MAVLink message 69 MANUAL_CONTROL it's using the full range
according to the message specs now [-1000,1000].
This commit is contained in:
Matthias Grob
2022-11-28 19:25:55 +01:00
parent 5579e319ff
commit 83246c84cf
13 changed files with 38 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+9 -18
View File
@@ -317,20 +317,20 @@ FixedwingPositionControl::manual_control_setpoint_poll()
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.z;
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_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
_manual_control_setpoint_for_height_rate = -_manual_control_setpoint.z;
_manual_control_setpoint_for_airspeed = _manual_control_setpoint.x;
}
// send neutral setpoints if no update for 1 s
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
_manual_control_setpoint_for_height_rate = 0.f;
_manual_control_setpoint_for_airspeed = 0.5f;
_manual_control_setpoint_for_airspeed = 0.f;
}
}
@@ -379,18 +379,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
// neutral throttle corresponds to trim airspeed
if (_manual_control_setpoint_for_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_for_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_for_airspeed * 2 - 1);
}
return math::interpolateNXY(_manual_control_setpoint_for_airspeed,
{-1.f, 0.f, 1.f},
{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
altctrl_airspeed = _commanded_manual_airspeed_setpoint;
@@ -1910,7 +1901,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
float throttle_max = _param_fw_thr_max.get();
// enable the operator to kill the throttle on ground
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -1958,7 +1949,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
float throttle_max = _param_fw_thr_max.get();
// enable the operator to kill the throttle on ground
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -2
View File
@@ -127,7 +127,7 @@ static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float THROTTLE_THRESH = 0.05f;
static constexpr float THROTTLE_THRESH = -.9f;
// [m/s/s] slew rate limit for airspeed setpoint changes
static constexpr float ASPD_SP_SLEW_RATE = 1.f;
@@ -293,7 +293,7 @@ private:
// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
float _manual_control_setpoint_for_height_rate{0.0f};
// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
float _manual_control_setpoint_for_airspeed{0.0f};
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED