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

…ghtTaskAuto

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -69,10 +69,15 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 	}
 
 	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
+	_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;
-
-	_unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity;
-	_want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f;
+
+	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 = _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();

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -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 @@ class FlightTaskAuto : public FlightTask
 	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,

src/modules/flight_mode_manager/tasks/Utility/Stop.cpp

@@ -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);
 }

src/modules/flight_mode_manager/tasks/Utility/Stop.hpp

@@ -42,6 +42,7 @@
 #include <matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+#include <px4_platform_common/log.h>
 
 using matrix::Vector3f;
 using matrix::Vector2f;
@@ -76,20 +77,31 @@ class Stop : public ModuleParams
 	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;
 
 	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,