Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

MultcopterPositionControl: fix executing a zero setpoint for 200ms

Browse Source 
This is a combination of the originally introduced delay:
06c10f61c1
then the emergency failsafe being changed to not just land,
position control being rescheduled to not overwrite every setpoint in:
e502214429
and it being fixed by overwriting the setpoint but not removing
the long obsolete hystersis here:
114e85d260
This commit is contained in:
Matthias Grob
2022-04-06 20:46:02 +02:00
parent cb484c5ac7
commit 3fb4889ab9
2 changed files with 41 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+17 -28
View File
@@ -51,7 +51,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
_vel_z_deriv(this, "VELD") _vel_z_deriv(this, "VELD")
{ {
parameters_update(true); parameters_update(true);
_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
_tilt_limit_slew_rate.setSlewRate(.2f); _tilt_limit_slew_rate.setSlewRate(.2f);
reset_setpoint_to_nan(_setpoint); reset_setpoint_to_nan(_setpoint);
_takeoff_status_pub.advertise(); _takeoff_status_pub.advertise();
@@ -402,7 +401,7 @@ void MulticopterPositionControl::Run()
if ((_setpoint.timestamp < _time_position_control_enabled) if ((_setpoint.timestamp < _time_position_control_enabled)
&& (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) {
failsafe(vehicle_local_position.timestamp_sample, _setpoint, states); _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states);
} }
} }
@@ -514,19 +513,11 @@ void MulticopterPositionControl::Run()
_control.setState(states); _control.setState(states);
// Run position control // Run position control
if (_control.update(dt)) { if (!_control.update(dt)) {
_failsafe_land_hysteresis.set_state_and_update(false, vehicle_local_position.timestamp_sample);
} else {
// Failsafe // Failsafe
vehicle_local_position_setpoint_s failsafe_setpoint{}; _vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints
failsafe(vehicle_local_position.timestamp_sample, failsafe_setpoint, states); _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states));
// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};
_control.setInputSetpoint(failsafe_setpoint);
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
_control.update(dt); _control.update(dt);
} }
@@ -566,7 +557,7 @@ void MulticopterPositionControl::Run()
perf_end(_cycle_perf); perf_end(_cycle_perf);
} }
void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
const PositionControlStates &states) const PositionControlStates &states)
{ {
// do not warn while we are disarmed, as we might not have valid setpoints yet // do not warn while we are disarmed, as we might not have valid setpoints yet
@@ -577,16 +568,13 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
_last_warn = now; _last_warn = now;
} }
// Only react after a short delay vehicle_local_position_setpoint_s failsafe_setpoint{};
_failsafe_land_hysteresis.set_state_and_update(true, now); reset_setpoint_to_nan(failsafe_setpoint);
failsafe_setpoint.timestamp = now;
if (_failsafe_land_hysteresis.get_state()) {
reset_setpoint_to_nan(setpoint);
setpoint.timestamp = now;
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
// don't move along xy // don't move along xy
setpoint.vx = setpoint.vy = 0.f; failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f;
if (warn) { if (warn) {
PX4_WARN("Failsafe: stop and wait"); PX4_WARN("Failsafe: stop and wait");
@@ -594,8 +582,8 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
} else { } else {
// descend with land speed since we can't stop // descend with land speed since we can't stop
setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f; failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f;
setpoint.vz = _param_mpc_land_speed.get(); failsafe_setpoint.vz = _param_mpc_land_speed.get();
if (warn) { if (warn) {
PX4_WARN("Failsafe: blind land"); PX4_WARN("Failsafe: blind land");
@@ -604,20 +592,21 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
if (PX4_ISFINITE(states.velocity(2))) { if (PX4_ISFINITE(states.velocity(2))) {
// don't move along z if we can stop in all dimensions // don't move along z if we can stop in all dimensions
if (!PX4_ISFINITE(setpoint.vz)) { if (!PX4_ISFINITE(failsafe_setpoint.vz)) {
setpoint.vz = 0.f; failsafe_setpoint.vz = 0.f;
} }
} else { } else {
// emergency descend with a bit below hover thrust // emergency descend with a bit below hover thrust
setpoint.vz = NAN; failsafe_setpoint.vz = NAN;
setpoint.acceleration[2] = .3f; failsafe_setpoint.acceleration[2] = .3f;
if (warn) { if (warn) {
PX4_WARN("Failsafe: blind descend"); PX4_WARN("Failsafe: blind descend");
} }
} }
}
return failsafe_setpoint;
} }
void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+4 -11
View File
@@ -42,7 +42,6 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/controllib/blocks.hpp> #include <lib/controllib/blocks.hpp>
#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/slew_rate/SlewRateYaw.hpp>
#include <lib/systemlib/mavlink_log.h> #include <lib/systemlib/mavlink_log.h>
@@ -189,15 +188,11 @@ private:
/** Timeout in us for trajectory data to get considered invalid */ /** Timeout in us for trajectory data to get considered invalid */
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
/** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */
static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms;
/** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off and tilt is limited */ /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off and tilt is limited */
static constexpr float ALTITUDE_THRESHOLD = 0.3f; static constexpr float ALTITUDE_THRESHOLD = 0.3f;
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 */
SlewRate<float> _tilt_limit_slew_rate; SlewRate<float> _tilt_limit_slew_rate;
uint8_t _vxy_reset_counter{0}; uint8_t _vxy_reset_counter{0};
@@ -221,13 +216,11 @@ private:
PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos);
/** /**
* Failsafe. * Generate setpoint to bridge no executable setpoint being available.
* If flighttask fails for whatever reason, then do failsafe. This could * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
* occur if the commander fails to switch to a mode in case of invalid states or * This should only happen briefly when transitioning and never during mode operation or by design.
* setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately.
*/ */
void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states); vehicle_local_position_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states);
/** /**
* Reset setpoints to NAN * Reset setpoints to NAN