Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 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
+37 -48
View File
@@ -51,7 +51,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
_vel_z_deriv(this, "VELD")
{
parameters_update(true);
_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
_tilt_limit_slew_rate.setSlewRate(.2f);
reset_setpoint_to_nan(_setpoint);
_takeoff_status_pub.advertise();
@@ -402,7 +401,7 @@ void MulticopterPositionControl::Run()
if ((_setpoint.timestamp < _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);
// Run position control
if (_control.update(dt)) {
_failsafe_land_hysteresis.set_state_and_update(false, vehicle_local_position.timestamp_sample);
} else {
if (!_control.update(dt)) {
// 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);
// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};
_control.setInputSetpoint(failsafe_setpoint);
_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states));
_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);
}
@@ -566,7 +557,7 @@ void MulticopterPositionControl::Run()
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)
{
// do not warn while we are disarmed, as we might not have valid setpoints yet
@@ -577,47 +568,45 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
_last_warn = now;
}
// Only react after a short delay
_failsafe_land_hysteresis.set_state_and_update(true, now);
vehicle_local_position_setpoint_s failsafe_setpoint{};
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))) {
// don't move along xy
failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f;
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
// don't move along xy
setpoint.vx = setpoint.vy = 0.f;
if (warn) {
PX4_WARN("Failsafe: stop and wait");
}
} else {
// descend with land speed since we can't stop
setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f;
setpoint.vz = _param_mpc_land_speed.get();
if (warn) {
PX4_WARN("Failsafe: blind land");
}
if (warn) {
PX4_WARN("Failsafe: stop and wait");
}
if (PX4_ISFINITE(states.velocity(2))) {
// don't move along z if we can stop in all dimensions
if (!PX4_ISFINITE(setpoint.vz)) {
setpoint.vz = 0.f;
}
} else {
// descend with land speed since we can't stop
failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f;
failsafe_setpoint.vz = _param_mpc_land_speed.get();
} else {
// emergency descend with a bit below hover thrust
setpoint.vz = NAN;
setpoint.acceleration[2] = .3f;
if (warn) {
PX4_WARN("Failsafe: blind descend");
}
if (warn) {
PX4_WARN("Failsafe: blind land");
}
}
if (PX4_ISFINITE(states.velocity(2))) {
// don't move along z if we can stop in all dimensions
if (!PX4_ISFINITE(failsafe_setpoint.vz)) {
failsafe_setpoint.vz = 0.f;
}
} else {
// emergency descend with a bit below hover thrust
failsafe_setpoint.vz = NAN;
failsafe_setpoint.acceleration[2] = .3f;
if (warn) {
PX4_WARN("Failsafe: blind descend");
}
}
return failsafe_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 <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <lib/systemlib/mavlink_log.h>
@@ -189,15 +188,11 @@ private:
/** Timeout in us for trajectory data to get considered invalid */
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 */
static constexpr float ALTITUDE_THRESHOLD = 0.3f;
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;
uint8_t _vxy_reset_counter{0};
@@ -221,13 +216,11 @@ private:
PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos);
/**
* Failsafe.
* If flighttask fails for whatever reason, then do failsafe. This could
* occur if the commander fails to switch to a mode in case of invalid states or
* setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
* to true, the failsafe will be initiated immediately.
* Generate setpoint to bridge no executable setpoint being available.
* Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
* This should only happen briefly when transitioning and never during mode operation or by design.
*/
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