From ddd6afa23833a415e2a23d7ec509900a44af5a9a Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 13 Dec 2024 16:15:32 +0100 Subject: [PATCH] MPC: Auto: Use the Stop Motion Primitive for emergency braking in FlightTaskAuto --- .../tasks/Auto/FlightTaskAuto.cpp | 106 +++++++++--------- .../tasks/Auto/FlightTaskAuto.hpp | 3 +- .../tasks/Utility/Stop.cpp | 17 ++- .../tasks/Utility/Stop.hpp | 11 ++ 4 files changed, 82 insertions(+), 55 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index fe34719c98c1..ce4423220ab0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/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(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23ad..21b270719b08 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/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) _param_mpc_xy_cruise, diff --git a/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp b/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp index df2e19422a9a..9c3fc307a7c5 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp +++ b/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); } diff --git a/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp b/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp index 3bcb95ac1d01..f3dbd85488f1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp @@ -77,7 +77,16 @@ 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; @@ -85,12 +94,14 @@ class Stop : public ModuleParams 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) _param_mpc_acc_down_max,