fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 17:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Expanded Vehicleconstraints to xy for emergency braking

Browse Source
This commit is contained in:
Claudio Chies 2024-10-01 11:14:07 +02:00
parent ca141642ac
commit b04b64c260
3 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/VehicleConstraints.msg
View File

@ -5,5 +5,6 @@ uint64 timestamp # time since system start (microseconds)
float32 speed_up # in meters/sec
float32 speed_down # in meters/sec
float32 speed_xy # in meters/sec
bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)

12
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -812,6 +812,12 @@ void FlightTaskAuto::_updateTrajConstraints()
_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
_position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
// Stretch the constraints of the velocity controller to leave some room for an additional
// correction required by the altitude/vertical position controller
_constraints.speed_down = 1.2f * _param_mpc_z_v_auto_dn.get();
_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
@ -823,6 +829,7 @@ void FlightTaskAuto::_updateTrajConstraints()
// 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);
} else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get();
@ -849,11 +856,6 @@ void FlightTaskAuto::_updateTrajConstraints()
_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
}
// Stretch the constraints of the velocity controller to leave some room for an additional
// correction required by the altitude/vertical position controller
_constraints.speed_down = math::max(_constraints.speed_down, 1.2f * _param_mpc_z_v_auto_dn.get());;
_constraints.speed_up = math::max(_constraints.speed_up, 1.2f * _param_mpc_z_v_auto_up.get());;
}
bool FlightTaskAuto::_highEnoughForLandingGear()

4
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -525,12 +525,12 @@ void MulticopterPositionControl::Run()
PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get());
const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
_param_mpc_z_vel_max_dn.get();
// Allow ramping from zero thrust on takeoff
const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
float max_speed_xy = _param_mpc_xy_vel_max.get();
float max_speed_xy = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_xy :
_param_mpc_xy_vel_max.get();
if (PX4_ISFINITE(vehicle_local_position.vxy_max)) {
max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max);