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

StrickAccelerationXY: name setpoints unambiguous

Browse Source
This commit is contained in:
Matthias Grob
2021-02-09 11:31:35 +01:00
committed by Lorenz Meier
parent ab03cd8bb9
commit 79739c5676
2 changed files with 18 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+15 -14
View File
@@ -51,12 +51,12 @@ StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) :
void StickAccelerationXY::resetPosition()
{
_position.setNaN();
_position_setpoint.setNaN();
}
void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
{
_velocity = velocity;
_velocity_setpoint = velocity;
}
void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
@@ -77,34 +77,35 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
// Map stick input to acceleration
Sticks::limitStickUnitLengthXY(stick_xy);
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
_acceleration = stick_xy.emult(acceleration_scale);
applyFeasibilityLimit(_acceleration, dt);
_acceleration_setpoint = stick_xy.emult(acceleration_scale);
applyFeasibilityLimit(_acceleration_setpoint, dt);
// Add drag to limit speed and brake again
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity);
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint);
// Don't allow the drag to change the sign of the velocity, otherwise we might get into oscillations around 0, due
// to discretization
if (_acceleration.norm_squared() < FLT_EPSILON && _velocity.norm_squared() < drag.norm_squared() * dt * dt) {
if (_acceleration_setpoint.norm_squared() < FLT_EPSILON
&& _velocity_setpoint.norm_squared() < drag.norm_squared() * dt * dt) {
drag.setZero();
_velocity.setZero();
_velocity_setpoint.setZero();
}
_acceleration -= drag;
_acceleration_setpoint -= drag;
applyTiltLimit(_acceleration);
applyTiltLimit(_acceleration_setpoint);
// Generate velocity setpoint by forward integrating commanded acceleration
_velocity += _acceleration * dt;
_velocity_setpoint += _acceleration_setpoint * dt;
lockPosition(_velocity, pos, dt, _position);
lockPosition(_velocity_setpoint, pos, dt, _position_setpoint);
}
void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
{
pos_sp.xy() = _position;
vel_sp.xy() = _velocity;
acc_sp.xy() = _acceleration;
pos_sp.xy() = _position_setpoint;
vel_sp.xy() = _velocity_setpoint;
acc_sp.xy() = _acceleration_setpoint;
}
void StickAccelerationXY::applyFeasibilityLimit(Vector2f &acceleration, const float dt)
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+3 -3
View File
@@ -71,9 +71,9 @@ private:
SlewRate<float> _acceleration_slew_rate_y;
AlphaFilter<float> _brake_boost_filter;
matrix::Vector2f _position;
matrix::Vector2f _velocity;
matrix::Vector2f _acceleration;
matrix::Vector2f _position_setpoint;
matrix::Vector2f _velocity_setpoint;
matrix::Vector2f _acceleration_setpoint;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,