From cac2e175ae4489fef166b7b3d87c9d7bf5d72036 Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 13 Apr 2024 12:45:00 +0900 Subject: [PATCH] Copter: No unnecessary operations --- ArduCopter/mode_poshold.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 7b47eeb7c3592a..680137ac32b3ae 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -528,15 +528,17 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel float brake_rate = g.poshold_brake_rate; brake_rate /= (float)LOOP_RATE_FACTOR; - if (brake_rate <= 1.0f) { + if (brake_rate < 1.0f) { brake_rate = 1.0f; } // calculate velocity-only based lean angle - if (velocity >= 0) { + if (is_positive(velocity)) { lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); - } else { + } else if (is_negative(velocity)) { lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); + } else { + lean_angle = 0.0f; } // do not let lean_angle be too far from brake_angle