Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

Make throttle stick scaling more predictable in mc-stabilized mode (#24710)

Browse Source 
* Rescale throttle curve to HTE as option

* Add slew rate to hover thrust estimate

* Update docs to reflect changes

Fix formating

* mc_att_control: suggestions for hover thrust slewing

- Slower slew rate
- Move update of hover thrust estimate into main loop
- Make sure dt for slew rate is correct
- Apply parameter updates if hover thrust estimate not used
- Parameter description in metadata files

* fix: Rescaling cases order

* Rescale to HTE estimate by default

* Update src/modules/mc_pos_control/multicopter_position_control_params.c

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

* Fix hte sub name

* Update MPC_THR_CURVE description

* Swap thr curve values to make hte rescaling default for value 0

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
dawr68
2025-04-22 16:36:33 +02:00
committed by GitHub
parent 9404783c99
commit f08d01b4d5
5 changed files with 55 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/en/advanced_config/parameter_reference.md
+5 -4
View File
@@ -24244,12 +24244,13 @@ Reboot | minValue | maxValue | increment | default | unit
Thrust curve mapping in Stabilized Mode.
Defines how the throttle stick is mapped to collective thrust in Stabilized mode. Rescale to hover thrust: Stick input is linearly rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). No Rescale: Directly map the stick 1:1 to the output. Can be useful with very low hover thrust which leads to much distortion and the upper half getting sensitive. With MPC_THR_HOVER 0.5 both modes are the same.
Defines how the throttle stick is mapped to collective thrust in Stabilized mode. Rescale to hover thrust estimate: Stick input is linearly rescaled, such that a centered throttle stick corresponds to the hover thrust estimator's output. No rescale: Directly map the stick 1:1 to the output. Can be useful with very low hover thrust which leads to much distortion and the upper half getting sensitive. Rescale to hover thrust parameter: Similar to rescaling to the hover thrust estimate, but it uses the hover thrust parameter value (see MPC_THR_HOVER) instead of estimated value. With MPC_THR_HOVER 0.5 it's equivalent to No rescale.
**Values:**
- `0`: Rescale to hover thrust
- `1`: No Rescale
- `0`: Rescale to estimate
- `1`: No rescale
- `2`: Rescale to parameter
Reboot | minValue | maxValue | increment | default | unit
@@ -24338,7 +24339,7 @@ Reboot | minValue | maxValue | increment | default | unit
Hover thrust estimator.
Disable to use the fixed parameter MPC_THR_HOVER Enable to use the hover thrust estimator
Disable to use the fixed parameter MPC_THR_HOVER. Enable to use the hover thrust estimator. This parameter does not influence Stabilized mode throttle curve (see MPC_THR_CURVE).
Reboot | minValue | maxValue | increment | default | unit
--- | --- | --- | --- | --- | ---
src/modules/mc_att_control/mc_att_control.hpp
+3 -1
View File
@@ -121,7 +121,9 @@ private:
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
float _hover_thrust{NAN};
float _hover_thrust_estimate{NAN};
SlewRate<float> _hover_thrust_slew_rate{.5f};
float _yaw_setpoint_stabilized{0.f};
bool _heading_good_for_control{true}; // initialized true to have heading lock when local position never published
src/modules/mc_att_control/mc_att_control_main.cpp
+35 -17
View File
@@ -65,6 +65,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
_manual_throttle_minimum.setSlewRate(0.05f);
// Rate of change 50% per second -> 2 seconds to ramp to 100%
_manual_throttle_maximum.setSlewRate(0.5f);
// Rate of change 5% per second -> 6 seconds to ramp 30% if hover thrust parameter is off
_hover_thrust_slew_rate.setSlewRate(0.05f);
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -95,6 +97,11 @@ MulticopterAttitudeControl::parameters_updated()
_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()),
radians(_param_mc_yawrate_max.get())));
// Update from hover thrust parameter if there's no valid estimate in use
if (!PX4_ISFINITE(_hover_thrust_estimate)) {
_hover_thrust_slew_rate.setForcedValue(_param_mpc_thr_hover.get());
}
_man_tilt_max = math::radians(_param_mpc_man_tilt_max.get());
}
@@ -103,31 +110,23 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
{
float thrust = 0.f;
{
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {
if (hte.valid) {
_hover_thrust = hte.hover_thrust;
}
}
}
if (!PX4_ISFINITE(_hover_thrust)) {
_hover_thrust = _param_mpc_thr_hover.get();
}
// throttle_stick_input is in range [-1, 1]
switch (_param_mpc_thr_curve.get()) {
case 1: // no rescaling to hover throttle
case 1: // no rescaling
thrust = math::interpolate(throttle_stick_input, -1.f, 1.f,
_manual_throttle_minimum.getState(), _param_mpc_thr_max.get());
break;
default: // 0 or other: rescale to hover throttle at 0 stick input
case 2: // rescale to hover thrust param at 0 stick input
thrust = math::interpolateNXY(throttle_stick_input,
{-1.f, 0.f, 1.f},
{_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()});
{_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
break;
default: // 0 or other: rescale to HTE value
thrust = math::interpolateNXY(throttle_stick_input,
{-1.f, 0.f, 1.f},
{_manual_throttle_minimum.getState(), _hover_thrust_slew_rate.getState(), _param_mpc_thr_max.get()});
break;
}
@@ -224,6 +223,21 @@ MulticopterAttitudeControl::Run()
parameters_updated();
}
// Update hover thrust for stick scaling
if (_vehicle_status_sub.updated()) {
hover_thrust_estimate_s hover_thrust_estimate;
if (_hover_thrust_estimate_sub.update(&hover_thrust_estimate)) {
if (hover_thrust_estimate.valid) {
_hover_thrust_estimate = math::constrain(hover_thrust_estimate.hover_thrust, .05f, .9f);
} else {
// Possibly bad estimate before it got invalid, slew back to parameter
_hover_thrust_estimate = _param_mpc_thr_hover.get();
}
}
}
// run controller on attitude updates
vehicle_attitude_s v_att;
@@ -371,6 +385,10 @@ MulticopterAttitudeControl::Run()
} else {
_manual_throttle_maximum.setForcedValue(0.f);
}
if (PX4_ISFINITE(_hover_thrust_estimate)) {
_hover_thrust_slew_rate.update(_hover_thrust_estimate, dt);
}
}
perf_end(_loop_perf);
src/modules/mc_pos_control/multicopter_position_control_params.c
+3 -3
View File
@@ -49,10 +49,10 @@
PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
/**
* Hover thrust estimator
* Use hover thrust estimate for altitude control
*
* Disable to use the fixed parameter MPC_THR_HOVER
* Enable to use the hover thrust estimator
* Disable to use the fixed parameter MPC_THR_HOVER instead of the hover thrust estimate in the position controller.
* This parameter does not influence Stabilized mode throttle curve (see MPC_THR_CURVE).
*
* @boolean
* @group Multicopter Position Control
src/modules/mc_pos_control/multicopter_stabilized_mode_params.c
+9 -6
View File
@@ -77,17 +77,20 @@ PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f);
*
* Defines how the throttle stick is mapped to collective thrust in Stabilized mode.
*
* Rescale to hover thrust:
* Stick input is linearly rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER).
* Rescale to hover thrust estimate:
* Stick input is linearly rescaled, such that a centered throttle stick corresponds to the hover thrust estimator's output.
*
* No Rescale:
* No rescale:
* Directly map the stick 1:1 to the output.
* Can be useful with very low hover thrust which leads to much distortion and the upper half getting sensitive.
*
* With MPC_THR_HOVER 0.5 both modes are the same.
* Rescale to hover thrust parameter:
* Similar to rescaling to the hover thrust estimate, but it uses the hover thrust parameter value (see MPC_THR_HOVER) instead of estimated value.
* With MPC_THR_HOVER 0.5 it's equivalent to No rescale.
*
* @value 0 Rescale to hover thrust
* @value 1 No Rescale
* @value 0 Rescale to estimate
* @value 1 No rescale
* @value 2 Rescale to parameter
* @group Multicopter Position Control
*/
PARAM_DEFINE_INT32(MPC_THR_CURVE, 0);