From 316e0dfeb5134fc4f67c860f2b7541e606eb9819 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 2 Jun 2021 16:32:25 +0200
Subject: [PATCH] AutoLineSmoothVel: add emergency braking mode

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.
---
 .../FlightTaskAutoLineSmoothVel.cpp           | 29 +++++++++++++++++--
 .../FlightTaskAutoLineSmoothVel.hpp           |  3 ++
 2 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index e5fd89e196..c9ad9b7b12 100644
--- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -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();
 
diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index 90eb84c68e..f7d4ec3367 100644
--- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -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();