From 9be8f81d754d71a1357e739a21cac2c3d9be1acb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 10 Mar 2023 08:17:20 -0500 Subject: [PATCH] flight_mode_manager: StickAccelerationXY protect from NAN velocity reset Co-authored-by: Matthias Grob --- .../FlightTaskManualAcceleration.cpp | 4 +--- .../tasks/Utility/StickAccelerationXY.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index aba0089ece..219868f64b 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -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; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index a3c95182be..d1b7743625 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -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); }