aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_L1_Control: Fixed formula in AP_L1_Control::nav_roll_cd

Browse Source 
See details here: https://github.com/ArduPilot/ardupilot/issues/24319
This commit is contained in:
abaghiyan 2023-07-17 13:04:48 +04:00 committed by Andrew Tridgell
parent dbf5bfd707
commit c2f4661b90
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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;
}