fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualAcceleration: fix velocity constraint overwriting + altitude limit slow down

Browse Source 
- The velocity constraint gets set from multiple places e.g. Position slow knob and altitude related slow down. Depending on the execution order it was overwritten with a higher value again not obeying a stricter limit.
- The slowdown used valocities as inputs instead of the ratio of available altitude.
This commit is contained in:
Pernilla 2025-09-10 10:18:09 +02:00 committed by GitHub
parent 63ec2f0406
commit b4395d5960
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@ -74,12 +74,11 @@ bool FlightTaskManualAcceleration::update()
static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
if (max_hagl_ratio > factor_threshold) {
max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
_stick_acceleration_xy.setVelocityConstraint(interpolate(max_hagl_ratio, factor_threshold, 1.f, vxy_max, min_vel));
} else {
_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
} else if (PX4_ISFINITE(vehicle_local_pos.vxy_max)) {
_stick_acceleration_xy.setVelocityConstraint(vehicle_local_pos.vxy_max);
}
_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,

25
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -43,6 +43,8 @@ StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) :
{
_brake_boost_filter.reset(1.f);
resetPosition();
_velocity_slew_rate_xy.setSlewRate(_param_mpc_acc_hor.get());
_velocity_slew_rate_xy.setForcedValue(_param_mpc_vel_manual.get());
}
void StickAccelerationXY::resetPosition()
@ -75,23 +77,22 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
}
}
void StickAccelerationXY::setVelocityConstraint(float vel)
{
if ((vel < _velocity_constraint) && (vel >= FLT_EPSILON)) {
_velocity_constraint = vel;
}
};
void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
const matrix::Vector2f &vel_sp_feedback, const float dt)
{
// gradually adjust velocity constraint because good tracking is required for the drag estimation
if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) {
if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) {
_current_velocity_constraint = _targeted_velocity_constraint;
} else {
_current_velocity_constraint = math::constrain(_targeted_velocity_constraint,
_current_velocity_constraint - dt * _param_mpc_acc_hor.get(),
_current_velocity_constraint + dt * _param_mpc_acc_hor.get());
}
}
// avoid setpoint steps from limit changes to improve velocity tracking and hence drag estimation
const float velocity_constraint = _velocity_slew_rate_xy.update(_velocity_constraint, dt);
_velocity_constraint = _param_mpc_vel_manual.get(); // reset, reduced to strictest limit in next loop
// maximum commanded velocity can be constrained dynamically
const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint);
const float velocity_sc = fminf(_param_mpc_vel_manual.get(), velocity_constraint);
Vector2f velocity_scale(velocity_sc, velocity_sc);
// maximum commanded acceleration is scaled down with velocity
const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());

10
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
View File

@ -64,8 +64,10 @@ public:
void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
float getMaxAcceleration() { return _param_mpc_acc_hor.get(); };
float getMaxJerk() { return _param_mpc_jerk_max.get(); };
void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
float getVelocityConstraint() { return _current_velocity_constraint; };
// Assuming the velocity constraint resets in every loop and update constraint if new value is lower
void setVelocityConstraint(float vel);
float getVelocityConstraint() { return _velocity_slew_rate_xy.getState(); };
private:
CollisionPrevention _collision_prevention{this};
@ -80,6 +82,7 @@ private:
SlewRate<float> _acceleration_slew_rate_x;
SlewRate<float> _acceleration_slew_rate_y;
SlewRate<float> _velocity_slew_rate_xy;
AlphaFilter<float> _brake_boost_filter;
matrix::Vector2f _position_setpoint;
@ -87,8 +90,7 @@ private:
matrix::Vector2f _acceleration_setpoint;
matrix::Vector2f _acceleration_setpoint_prev;
float _targeted_velocity_constraint{INFINITY};
float _current_velocity_constraint{INFINITY};
float _velocity_constraint{INFINITY};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,