From 710f977349416e382f799a53212d67d61c26c4b7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 30 Aug 2023 15:18:48 +0200 Subject: [PATCH] mc_att_control: ramp thrust in after spoolup time --- src/modules/mc_att_control/mc_att_control.hpp | 1 + .../mc_att_control/mc_att_control_main.cpp | 28 +++++++++++++------ 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 612b6f083e..31ca3eafa0 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -122,6 +122,7 @@ private: float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air + SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; 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 2233bc5669..2808084d69 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,6 +63,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : parameters_updated(); // Rate of change 5% per second -> 1.6 seconds to ramp to default 8% MPC_MANTHR_MIN _manual_throttle_minimum.setSlewRate(0.05f); + // Rate of change 50% per second -> 2 seconds to ramp to 100% + _manual_throttle_maximum.setSlewRate(0.5f); } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -99,14 +101,21 @@ MulticopterAttitudeControl::parameters_updated() float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { - // throttle_stick_input is in range [0, 1] + float thrust = 0.f; + switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return math::interpolate(throttle_stick_input, 0.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); + 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.5 stick - return math::interpolateN(throttle_stick_input, {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + break; } + + return math::min(thrust, _manual_throttle_maximum.getState()); } void @@ -179,13 +188,9 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, attitude_setpoint.pitch_body = euler_sp(1); attitude_setpoint.yaw_body = euler_sp(2); - // Only set thrust setpoint different from idle once the vehicle is spooled up - if (_spooled_up) { - attitude_setpoint.thrust_body[2] = -throttle_curve((_manual_control_setpoint.throttle + 1.f) * .5f); - } + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); attitude_setpoint.timestamp = hrt_absolute_time(); - _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); // update attitude controller setpoint immediately @@ -346,6 +351,13 @@ MulticopterAttitudeControl::Run() _manual_throttle_minimum.update(_param_mpc_manthr_min.get(), dt); } + if (_spooled_up) { + _manual_throttle_maximum.update(1.f, dt); + + } else { + _manual_throttle_maximum.setForcedValue(0.f); + } + // reset yaw setpoint during transitions, tailsitter.cpp generates // attitude setpoint for the transition _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control || (_vtol && _vtol_in_transition_mode);