From 8a5556cb4efc395262c8bbe0b51b4c7ef30b92be Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 5 Oct 2024 11:06:54 +0900 Subject: [PATCH] Copter: Consolidate processing --- ArduCopter/mode_poshold.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index c06072c597..40a86a8cff 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -535,11 +535,7 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel } // calculate velocity-only based lean angle - if (velocity >= 0) { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); - } else { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); - } + lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (fabsf(velocity) + 60.0f)); // do not let lean_angle be too far from brake_angle brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate);