From 24111df17635ecf22e0c62f2c411278f9522a990 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 3 Feb 2023 14:32:40 +0100 Subject: [PATCH] mc_att_control_main: zero minimal thrust when landed but this time compared to 2fbb70d9ca94f442de1345221be18ba738cda952 the minimum thrust is ramping to not produce a thrust jump. The better long term solution will be to always have airmode but with the effect limited to a magnitude of MPC_MANTHR_MIN. --- src/lib/slew_rate/SlewRate.hpp | 1 + src/modules/mc_att_control/mc_att_control.hpp | 5 +++++ .../mc_att_control/mc_att_control_main.cpp | 22 ++++++++++++++++--- 3 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/lib/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp index 945276fedc..363f2d2cc7 100644 --- a/src/lib/slew_rate/SlewRate.hpp +++ b/src/lib/slew_rate/SlewRate.hpp @@ -49,6 +49,7 @@ class SlewRate { public: SlewRate() = default; + SlewRate(Type initial_value) { setForcedValue(initial_value); } ~SlewRate() = default; /** diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ca4482d487..a182602e37 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -50,10 +50,12 @@ #include #include #include +#include #include #include #include #include +#include #include @@ -100,6 +102,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -118,12 +121,14 @@ private: float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ 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 AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; + bool _landed{true}; bool _reset_yaw_sp{true}; bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published bool _vehicle_type_rotary_wing{true}; 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 5925237219..6102092c95 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,8 +60,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _vtol(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); } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -101,10 +102,10 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) // throttle_stick_input is in range [0, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get()); + return math::interpolate(throttle_stick_input, 0.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); default: // 0 or other: rescale to hover throttle at 0.5 stick - return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + return math::interpolateN(throttle_stick_input, {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); } } @@ -265,6 +266,14 @@ MulticopterAttitudeControl::Run() } } + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } + if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position; @@ -324,6 +333,13 @@ MulticopterAttitudeControl::Run() _vehicle_rates_setpoint_pub.publish(rates_setpoint); } + if (_landed) { + _manual_throttle_minimum.update(0.f, dt); + + } else { + _manual_throttle_minimum.update(_param_mpc_manthr_min.get(), dt); + } + // 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);