fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 15:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

MPC: Auto: Use the Stop Motion Primitive for emergency braking in FlightTaskAuto

Browse Source
This commit is contained in:
Claudio Chies 2024-12-13 16:15:32 +01:00
parent 4d003fd782
commit 1ee6d8b801
4 changed files with 84 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -49,10 +49,6 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
// Set setpoints equal current state.
_velocity_setpoint = _velocity;
_position_setpoint = _position;
Vector3f vel_prev{last_setpoint.velocity};
Vector3f pos_prev{last_setpoint.position};
Vector3f accel_prev{last_setpoint.acceleration};
@ -70,9 +66,18 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
if (_stop.checkMaxVelocityLimit(vel_prev, 1.3f)) {
PX4_WARN("Exceeded velocity limits, stopping vehicle");
_stop.initialize(accel_prev, vel_prev, pos_prev, _deltatime);
_jerk_setpoint = _stop.getJerkSetpoint();
_acceleration_setpoint = _stop.getAccelerationSetpoint();
_velocity_setpoint = _stop.getVelocitySetpoint();
_position_setpoint = _stop.getPositionSetpoint();
}
_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
_updateTrajConstraints();
_is_emergency_braking_active = false;
_time_last_cruise_speed_override = 0;
return ret;
@ -172,25 +177,50 @@ bool FlightTaskAuto::update()
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first)
&& !_yaw_sp_aligned;
const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active;
_updateTrajConstraints();
PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
_position_smoothing.generateSetpoints(
_position,
waypoints,
_velocity_setpoint,
_deltatime,
force_zero_velocity_setpoint,
smoothed_setpoints
);
_jerk_setpoint = smoothed_setpoints.jerk;
_acceleration_setpoint = smoothed_setpoints.acceleration;
_velocity_setpoint = smoothed_setpoints.velocity;
_position_setpoint = smoothed_setpoints.position;
if (_stop.isActive()) {
_stop.update(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime);
_jerk_setpoint = _stop.getJerkSetpoint();
_acceleration_setpoint = _stop.getAccelerationSetpoint();
_velocity_setpoint = _stop.getVelocitySetpoint();
_position_setpoint = _stop.getPositionSetpoint();
_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
_want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f;
_unsmoothed_velocity_setpoint = _stop.getUnsmoothedVelocity();
_yaw_setpoint = _stop.getYaw();
} else if (_stop.wasActive()) {
_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
// Modifying the triplet outside of the navigatior is not ideal, but this is to catch an edge case.
_triplet_prev_wp = _position;
} else {
// Generate setpoints
PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
_position_smoothing.generateSetpoints(
_position,
waypoints,
_velocity_setpoint,
_deltatime,
should_wait_for_yaw_align,
smoothed_setpoints
);
_jerk_setpoint = smoothed_setpoints.jerk;
_acceleration_setpoint = smoothed_setpoints.acceleration;
_velocity_setpoint = smoothed_setpoints.velocity;
_position_setpoint = smoothed_setpoints.position;
_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
}
_want_takeoff = _unsmoothed_velocity_setpoint(2) < -0.3f;
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
@ -749,26 +779,12 @@ void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskAuto::_checkEmergencyBraking()
{
if (!_is_emergency_braking_active) {
if (!_stop.isActive()
&& _stop.checkMaxVelocityLimit(_velocity_setpoint,
1.3f)) { // the Factor 1.3 is taken over from the original PR (https://github.com/PX4/PX4-Autopilot/pull/18740)
// activate emergency braking if significantly outside of velocity bounds
const float factor = 1.3f;
const bool is_vertical_speed_exceeded = _position_smoothing.getCurrentVelocityZ() >
(factor * _param_mpc_z_vel_max_dn.get())
|| _position_smoothing.getCurrentVelocityZ() < -(factor * _param_mpc_z_vel_max_up.get());
const bool is_horizontal_speed_exceeded = _position_smoothing.getCurrentVelocityXY().longerThan(
factor * _param_mpc_xy_vel_max.get());
if (is_vertical_speed_exceeded || is_horizontal_speed_exceeded) {
_is_emergency_braking_active = true;
}
} else {
// deactivate emergency braking when the vehicle has come to a full stop
if (_position_smoothing.getCurrentVelocityZ() < 0.01f
&& _position_smoothing.getCurrentVelocityZ() > -0.01f
&& !_position_smoothing.getCurrentVelocityXY().longerThan(0.01f)) {
_is_emergency_braking_active = false;
}
_stop.initialize(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime);
_stop.setYaw(_yaw);
}
}
@ -818,18 +834,8 @@ void FlightTaskAuto::_updateTrajConstraints()
_constraints.speed_up = 1.2f * _param_mpc_xy_vel_max.get();
_constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();
if (_is_emergency_braking_active) {
// When initializing with large velocity, allow 1g of
// acceleration in 1s on all axes for fast braking
_position_smoothing.setMaxAcceleration({CONSTANTS_ONE_G, CONSTANTS_ONE_G, CONSTANTS_ONE_G});
_position_smoothing.setMaxJerk(CONSTANTS_ONE_G);
// If the current velocity is beyond the usual constraints, tell
// the controller to exceptionally increase its saturations to avoid
// cutting out the feedforward
_constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down);
_constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_up);
_constraints.speed_xy = math::max(_position_smoothing.getCurrentVelocityXY().norm(), _constraints.speed_xy);
if (_stop.isActive()) {
_stop.getConstraints(_constraints);
} else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get();

3
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -52,6 +52,7 @@
#include "Sticks.hpp"
#include "StickAccelerationXY.hpp"
#include "StickYaw.hpp"
#include "Stop.hpp"
// TODO: make this switchable in the board config, like a module
#if CONSTRAINED_FLASH
@ -157,8 +158,8 @@ protected:
matrix::Vector3f _land_position;
float _land_heading;
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _is_emergency_braking_active{false};
bool _want_takeoff{false};
Stop _stop{this};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,

17
src/modules/flight_mode_manager/tasks/Utility/Stop.cpp
View File

@ -67,7 +67,7 @@ void Stop::getConstraints(vehicle_constraints_s &constraints)
_position_smoothing.setMaxAccelerationXY(CONSTANTS_ONE_G);
_position_smoothing.setMaxAccelerationZ(2 * CONSTANTS_ONE_G);
_position_smoothing.setMaxJerk(CONSTANTS_ONE_G);
_position_smoothing.setMaxJerkZ(10.f * CONSTANTS_ONE_G); // Jerk in Z direction is only limited by motor inertia.
_position_smoothing.setMaxJerkZ(CONSTANTS_ONE_G);
}
}
@ -76,10 +76,12 @@ Stop::initialize(const Vector3f &acceleration, const Vector3f &velocity, const V
const float &deltatime)
{
if (velocity.isAllNan() || position.isAllNan() || acceleration.isAllNan()) {
PX4_ERR("Initialize stop with valid values");
PX4_ERR("Initialized stop with invalid values");
}
_isActive = true;
_wasActive = false;
_position_smoothing.reset(acceleration, velocity, position);
update(acceleration, velocity, position, deltatime);
}
@ -93,7 +95,9 @@ Stop::update(const Vector3f &acceleration, const Vector3f &velocity, const Vecto
} else if (_position_smoothing.getCurrentVelocityZ() < 0.01f
&& _position_smoothing.getCurrentVelocityZ() > -0.01f
&& !_position_smoothing.getCurrentVelocityXY().longerThan(0.01f)) {
// deactivate when the vehicle has come to a full stop
_isActive = false;
_wasActive = true;
_stop_position = position;
}
@ -117,9 +121,14 @@ Stop::update(const Vector3f &acceleration, const Vector3f &velocity, const Vecto
bool
Stop::checkMaxVelocityLimit(const Vector3f &velocity, const float &factor)
{
const bool exceeded_vel_z = fabsf(velocity(2)) > math::max(_param_mpc_z_vel_max_dn.get(),
_param_mpc_z_vel_max_up.get());
const bool exceeded_vel_z = velocity(2) > (factor * _param_mpc_z_vel_max_dn.get())
|| velocity(2) < -(factor * _param_mpc_z_vel_max_up.get());
const bool exceeded_vel_xy = velocity.xy().norm() > _param_mpc_xy_vel_max.get();
if ((exceeded_vel_xy || exceeded_vel_z)) {
PX4_DEBUG("exceeded_vel_xy: %d, exceeded_vel_z: %d", exceeded_vel_xy, exceeded_vel_z);
}
return (exceeded_vel_xy || exceeded_vel_z);
}

11
src/modules/flight_mode_manager/tasks/Utility/Stop.hpp
View File

@ -77,7 +77,16 @@ public:
Vector3f getUnsmoothedVelocity() const { return _unsmoothed_velocity; }
Vector3f getStopPosition() const { return _stop_position; }
void setYaw(float yaw) { _yaw_setpoint = yaw; }
float getYaw() const { return _yaw_setpoint; }
bool isActive() const {return _isActive;};
bool wasActive()
{
bool curr_state = _wasActive;
_wasActive = false;
return curr_state;
};
private:
PositionSmoothing _position_smoothing;
@ -85,12 +94,14 @@ private:
Vector3f _stop_position;
bool _exceeded_max_vel{false}; // true if we exceed the maximum velcoity of the auto flight task.
bool _isActive{false}; // true if the condition for braking is still valid
bool _wasActive{false}; // true if the condition for braking was valid in the previous iteration
Vector3f _jerk_setpoint;
Vector3f _acceleration_setpoint;
Vector3f _velocity_setpoint;
Vector3f _position_setpoint;
Vector3f _unsmoothed_velocity;
float _yaw_setpoint{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,