Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

StickAccelerationXY: access member setpoints directly

Browse Source 
This is a leftover from before converting the logic into
a completely separate class.
This commit is contained in:
Matthias Grob
2021-02-08 17:20:44 +01:00
committed by Lorenz Meier
parent 79739c5676
commit c9eac29d25
2 changed files with 12 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+10 -10
View File
@@ -78,7 +78,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
Sticks::limitStickUnitLengthXY(stick_xy);
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
_acceleration_setpoint = stick_xy.emult(acceleration_scale);
applyFeasibilityLimit(_acceleration_setpoint, dt);
applyFeasibilityLimit(dt);
// Add drag to limit speed and brake again
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint);
@@ -98,7 +98,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
// Generate velocity setpoint by forward integrating commanded acceleration
_velocity_setpoint += _acceleration_setpoint * dt;
lockPosition(_velocity_setpoint, pos, dt, _position_setpoint);
lockPosition(pos, dt);
}
void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
@@ -108,13 +108,13 @@ void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vecto
acc_sp.xy() = _acceleration_setpoint;
}
void StickAccelerationXY::applyFeasibilityLimit(Vector2f &acceleration, const float dt)
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());
acceleration(0) = _acceleration_slew_rate_x.update(acceleration(0), dt);
acceleration(1) = _acceleration_slew_rate_y.update(acceleration(1), dt);
_acceleration_setpoint(0) = _acceleration_slew_rate_x.update(_acceleration_setpoint(0), dt);
_acceleration_setpoint(1) = _acceleration_slew_rate_y.update(_acceleration_setpoint(1), dt);
}
Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const float dt, const Vector2f &stick_xy,
@@ -147,15 +147,15 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
}
}
void StickAccelerationXY::lockPosition(const Vector2f &vel_sp, const Vector3f &pos, const float dt, Vector2f &pos_sp)
void StickAccelerationXY::lockPosition(const Vector3f &pos, const float dt)
{
if (vel_sp.norm_squared() < FLT_EPSILON) {
if (!PX4_ISFINITE(pos_sp(0))) {
pos_sp = Vector2f(pos);
if (_velocity_setpoint.norm_squared() < FLT_EPSILON) {
if (!PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint = Vector2f(pos);
}
} else {
pos_sp.setNaN();
_position_setpoint.setNaN();
}
}
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+2 -3
View File
@@ -60,12 +60,11 @@ public:
void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
private:
void applyFeasibilityLimit(matrix::Vector2f &acceleration, const float dt);
void applyFeasibilityLimit(const float dt);
matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
const matrix::Vector2f &vel_sp);
void applyTiltLimit(matrix::Vector2f &acceleration);
void lockPosition(const matrix::Vector2f &vel_sp, const matrix::Vector3f &pos, const float dt,
matrix::Vector2f &pos_sp);
void lockPosition(const matrix::Vector3f &pos, const float dt);
SlewRate<float> _acceleration_slew_rate_x;
SlewRate<float> _acceleration_slew_rate_y;