fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

StickAcceleration: synchronize XY acceleration profiles

Browse Source
This commit is contained in:
bresch
2021-03-19 13:51:15 +01:00
committed by Matthias Grob
parent c8c3f57ba1
commit 6fda6e43af
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+8 -2
View File
@@ -99,6 +99,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
_velocity_setpoint += _acceleration_setpoint * dt;
lockPosition(pos, vel_sp_feedback, dt);
_acceleration_setpoint_prev = _acceleration_setpoint;
}
void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
@@ -111,8 +112,13 @@ void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vecto
void StickAccelerationXY::applyFeasibilityLimit(const float dt)
{
// Apply jerk limit - acceleration slew rate
_acceleration_slew_rate_x.setSlewRate(_param_mpc_jerk_max.get());
_acceleration_slew_rate_y.setSlewRate(_param_mpc_jerk_max.get());
// Scale each jerk limit with the normalized projection of the acceleration
// setpoint increment to produce a synchronized motion
const Vector2f dir = Vector2f(_acceleration_setpoint - _acceleration_setpoint_prev).unit_or_zero();
const float jerk_max_x = fabsf(dir(0)) * _param_mpc_jerk_max.get();
const float jerk_max_y = fabsf(dir(1)) * _param_mpc_jerk_max.get();
_acceleration_slew_rate_x.setSlewRate(jerk_max_x);
_acceleration_slew_rate_y.setSlewRate(jerk_max_y);
_acceleration_setpoint(0) = _acceleration_slew_rate_x.update(_acceleration_setpoint(0), dt);
_acceleration_setpoint(1) = _acceleration_slew_rate_y.update(_acceleration_setpoint(1), dt);
}
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+1
View File
@@ -73,6 +73,7 @@ private:
matrix::Vector2f _position_setpoint;
matrix::Vector2f _velocity_setpoint;
matrix::Vector2f _acceleration_setpoint;
matrix::Vector2f _acceleration_setpoint_prev;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,