From ae1eb93aeca49315215fc1e9b6a47bef3b8c0c40 Mon Sep 17 00:00:00 2001 From: abaghiyan Date: Fri, 21 Jul 2023 04:58:45 +0400 Subject: [PATCH] AP_L1_Control: Made changes to avoid zero division in proposed formula According suggestion from Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397 --- libraries/AP_L1_Control/AP_L1_Control.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 7805a3d2f7..b15bdc9122 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -84,9 +84,12 @@ int32_t AP_L1_Control::nav_roll_cd(void) const liftForce * cos(roll) = gravityForce * cos(pitch); liftForce * sin(roll) = gravityForce * lateralAcceleration / gravityAcceleration; // as mass = gravityForce/gravityAcceleration see issue 24319 [https://github.com/ArduPilot/ardupilot/issues/24319] - Multiplier 100.0f is for converting degrees to centidegrees - */ - ret = degrees(atanf(_latAccDem * (1.0f/(GRAVITY_MSS * cosf(_ahrs.pitch))))) * 100.0f; + Multiplier 100.0f is for converting degrees to centidegrees + Made changes to avoid zero division as proposed by Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397 + */ + float pitchLimL1 = radians(60); // Suggestion: constraint may be modified to pitch limits if their absolute values are less than 90 degree and more than 60 degrees. + float pitchL1 = constrain_float(_ahrs.pitch,-pitchLimL1,pitchLimL1); + ret = degrees(atanf(_latAccDem * (1.0f/(GRAVITY_MSS * cosf(pitchL1))))) * 100.0f; ret = constrain_float(ret, -9000, 9000); return ret; }