Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: Made changes to avoid zero division in proposed formula

Browse Source 
According suggestion from Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397
This commit is contained in:
abaghiyan 2023-07-21 04:58:45 +04:00 committed by Andrew Tridgell
parent c2f4661b90
commit ae1eb93aec
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -85,8 +85,11 @@ int32_t AP_L1_Control::nav_roll_cd(void) const
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
Made changes to avoid zero division as proposed by Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397
*/
ret = degrees(atanf(_latAccDem * (1.0f/(GRAVITY_MSS * cosf(_ahrs.pitch))))) * 100.0f;
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;
}