fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

AutoLineSmoothVel: add emergency braking mode

Browse Source 
Currently only for the Z axis. If the current downward velocity is more
than twice the maximum allowed value, the emergency braking mode is
activated. In this mode, a higher vertical acceleration and jerk is used
until the vehicle stops moving.
This commit is contained in:
bresch 2021-06-02 16:32:25 +02:00 committed by Beat Küng
parent 92696b589f
commit 316e0dfeb5
2 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -66,6 +66,7 @@ bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint
_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
_updateTrajConstraints();
_is_emergency_braking_active = false;
return ret;
}
@ -116,6 +117,7 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
{
_checkEmergencyBraking();
_updateTurningCheck();
_prepareSetpoints();
_generateTrajectory();
@ -126,6 +128,20 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
}
}
void FlightTaskAutoLineSmoothVel::_checkEmergencyBraking()
{
if (!_is_emergency_braking_active) {
if (_trajectory[2].getCurrentVelocity() > (2.f * _param_mpc_z_vel_max_dn.get())) {
_is_emergency_braking_active = true;
}
} else {
if (fabsf(_trajectory[2].getCurrentVelocity()) < 0.01f) {
_is_emergency_braking_active = false;
}
}
}
void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
{
const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
@ -309,8 +325,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
_want_takeoff = false;
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
if (should_wait_for_yaw_align || _is_emergency_braking_active) {
_velocity_setpoint.setAll(0.f);
return;
}
@ -412,7 +429,13 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
_trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get());
_trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get());
if (_velocity_setpoint(2) < 0.f) { // up
if (_is_emergency_braking_active) {
// When initializing with large downward velocity, allow 1g of vertical
// acceleration for fast braking
_trajectory[2].setMaxAccel(9.81f);
_trajectory[2].setMaxJerk(9.81f);
} else if (_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get();
float z_vel_constraint = _param_mpc_z_vel_max_up.get();

3
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -63,6 +63,7 @@ protected:
void _generateSetpoints() override; /**< Generate setpoints along line. */
void _generateHeading();
void _checkEmergencyBraking();
void _updateTurningCheck();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
@ -80,6 +81,8 @@ protected:
float _max_speed_prev{};
bool _is_turning{false};
bool _is_emergency_braking_active{false};
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();
void _generateTrajectory();