forked from Archive/PX4-Autopilot
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:
parent
7239e8473e
commit
24111df176
|
@ -49,6 +49,7 @@ class SlewRate
|
|||
{
|
||||
public:
|
||||
SlewRate() = default;
|
||||
SlewRate(Type initial_value) { setForcedValue(initial_value); }
|
||||
~SlewRate() = default;
|
||||
|
||||
/**
|
||||
|
|
|
@ -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};
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue