Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MPC: Auto: Use the Stop Motion Primitive for emergency braking in FlighttaskAuto #24114

Draft
wants to merge 1 commit into
base: pr-flight_task_land
Choose a base branch
from
+86 −59
Draft

MPC: Auto: Use the Stop Motion Primitive for emergency braking in FlighttaskAuto #24114

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
114 changes: 60 additions & 54 deletions src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Important thing to look at during review, Triplets are usually not to be adjusted outside the navigator, but this was the only way I found for the drone to move in a direct line from the end position of the slow-down to the waypoint.


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

Expand Down Expand Up @@ -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();
Expand Down
3 changes: 2 additions & 1 deletion src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down
17 changes: 13 additions & 4 deletions src/modules/flight_mode_manager/tasks/Utility/Stop.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

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

Expand All @@ -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 changes: 11 additions & 0 deletions src/modules/flight_mode_manager/tasks/Utility/Stop.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,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,
Expand Down
Loading