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

flight_mode_manager: StickAccelerationXY protect from NAN velocity reset

Browse Source 
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2023-03-10 08:17:20 -05:00
committed by GitHub
co-authored by Matthias Grob
parent 435c799f57
commit 9be8f81d75
2 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
+1 -3
View File
@@ -52,9 +52,7 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
_stick_acceleration_xy.resetVelocity(_velocity.xy());
}
if (Vector2f(last_setpoint.acceleration).isAllFinite()) {
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
}
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
return ret;
}
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+8 -4
View File
@@ -57,13 +57,17 @@ void StickAccelerationXY::resetPosition(const matrix::Vector2f &position)
void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
{
_velocity_setpoint = velocity;
if (velocity.isAllFinite()) {
_velocity_setpoint = velocity;
}
}
void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
{
_acceleration_slew_rate_x.setForcedValue(acceleration(0));
_acceleration_slew_rate_y.setForcedValue(acceleration(1));
if (acceleration.isAllFinite()) {
_acceleration_slew_rate_x.setForcedValue(acceleration(0));
_acceleration_slew_rate_y.setForcedValue(acceleration(1));
}
}
void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
@@ -150,7 +154,7 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo
drag_coefficient *= _brake_boost_filter.getState();
// increase drag with sqareroot function when velocity is lower than 1m/s
// increase drag with squareroot function when velocity is lower than 1m/s
const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
return drag_coefficient.emult(velocity_with_sqrt_boost);
}