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

mc_att_control: add gradual3 function to cover hover thrust rescaling

Browse Source
This commit is contained in:
Matthias Grob
2020-07-15 14:58:37 +02:00
parent aa7735c1c0
commit 0b391fdcfc
2 changed files with 29 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+4 -9
View File
@@ -107,7 +107,7 @@ MulticopterAttitudeControl::parameters_updated()
float
MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
{
float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get();
const float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get();
// throttle_stick_input is in range [0, 1]
switch (_param_mpc_thr_curve.get()) {
@@ -115,14 +115,9 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
return throttle_min + throttle_stick_input * (_param_mpc_thr_max.get() - throttle_min);
default: // 0 or other: rescale to hover throttle at 0.5 stick
if (throttle_stick_input < 0.5f) {
return (_param_mpc_thr_hover.get() - throttle_min) / 0.5f * throttle_stick_input +
throttle_min;
} else {
return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) +
_param_mpc_thr_max.get();
}
return math::gradual3(throttle_stick_input,
0.f, .5f, 1.f,
throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
}
}