From c2f4661b90c9ef47ea06a1925f5ed72ee0835981 Mon Sep 17 00:00:00 2001 From: abaghiyan Date: Mon, 17 Jul 2023 13:04:48 +0400 Subject: [PATCH] AP_L1_Control: Fixed formula in AP_L1_Control::nav_roll_cd See details here: https://github.com/ArduPilot/ardupilot/issues/24319 --- libraries/AP_L1_Control/AP_L1_Control.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 3b9bbcab18..7805a3d2f7 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -79,7 +79,14 @@ int32_t AP_L1_Control::get_yaw_sensor() const int32_t AP_L1_Control::nav_roll_cd(void) const { float ret; - ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * (1.0f/GRAVITY_MSS)) * 100.0f); + /* + formula can be obtained through equations of balanced spiral: + 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; ret = constrain_float(ret, -9000, 9000); return ret; }