diff --git a/docs/en/advanced_config/parameter_reference.md b/docs/en/advanced_config/parameter_reference.md index 60f98bcc17..c77f7fc639 100644 --- a/docs/en/advanced_config/parameter_reference.md +++ b/docs/en/advanced_config/parameter_reference.md @@ -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 --- | --- | --- | --- | --- | --- diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index f2632279ab..4743d1936d 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -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 _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 diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 15f3f6ef40..7ab4f66682 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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); diff --git a/src/modules/mc_pos_control/multicopter_position_control_params.c b/src/modules/mc_pos_control/multicopter_position_control_params.c index 94435b1b33..9409065d7c 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_params.c @@ -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 diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index 50cf5a6536..6229b18bc5 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -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);