fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

MulticopterPositionControl: fix twitch on tilt limit relaxation

Browse Source
This commit is contained in:
Matthias Grob
2021-03-31 10:03:08 +02:00
parent 34a5948692
commit 94469d84ae
3 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/CMakeLists.txt
+1
View File
@@ -47,4 +47,5 @@ px4_add_module(
controllib controllib
git_ecl git_ecl
ecl_geo ecl_geo
SlewRate
) )
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+6 -8
View File
@@ -49,12 +49,9 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
_vel_y_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD") _vel_z_deriv(this, "VELD")
{ {
// fetch initial parameter values
parameters_update(true); parameters_update(true);
// set failsafe hysteresis
_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
_tilt_limit_slew_rate.setSlewRate(.2f);
reset_setpoint_to_nan(_setpoint); reset_setpoint_to_nan(_setpoint);
} }
@@ -378,13 +375,14 @@ void MulticopterPositionControl::Run()
} }
// limit tilt during takeoff ramupup // limit tilt during takeoff ramupup
if (_takeoff.getTakeoffState() < TakeoffState::flight) { float tilt_limit = math::radians(_param_mpc_tiltmax_air.get());
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_lnd.get()));
} else { if (_takeoff.getTakeoffState() < TakeoffState::flight) {
_control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get())); tilt_limit = math::radians(_param_mpc_tiltmax_lnd.get());
} }
_control.setTiltLimit(_tilt_limit_slew_rate.update(tilt_limit, dt));
const float speed_up = _takeoff.updateRamp(dt, const float speed_up = _takeoff.updateRamp(dt,
PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get());
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+2
View File
@@ -44,6 +44,7 @@
#include <lib/controllib/blocks.hpp> #include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h> #include <lib/hysteresis/hysteresis.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <lib/systemlib/mavlink_log.h> #include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h> #include <px4_platform_common/defines.h>
@@ -197,6 +198,7 @@ private:
static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf
systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
SlewRate<float> _tilt_limit_slew_rate;
uint8_t _old_takeoff_state{}; uint8_t _old_takeoff_state{};