mc_att_control_main: zero minimal thrust when landed

but this time compared to
2fbb70d9ca
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.
This commit is contained in:
Matthias Grob 2023-02-03 14:32:40 +01:00
parent 7239e8473e
commit 24111df176
3 changed files with 25 additions and 3 deletions

View File

@ -49,6 +49,7 @@ class SlewRate
{
public:
SlewRate() = default;
SlewRate(Type initial_value) { setForcedValue(initial_value); }
~SlewRate() = default;
/**

View File

@ -50,10 +50,12 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <AttitudeControl.hpp>
@ -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<float> _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air
AlphaFilter<float> _man_roll_input_filter;
AlphaFilter<float> _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};

View File

@ -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);