dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Differential Drive: Improved logic, to be independent of the heading gain

This commit is contained in:
PerFrivik 2024-03-25 10:58:35 +01:00
parent d0aab458e5
commit 83d91c1489
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
View File

@ -86,10 +86,11 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
}
float minTurningSpeed = _param_rdd_min_turning_speed.get();
float headingPGain = _param_rdd_p_gain_heading.get();
// Make sure we do not get stuck while turning as the error gets too small
if (fabsf(_heading_error) < minTurningSpeed) {
_heading_error = (_heading_error > 0) ? minTurningSpeed : -minTurningSpeed;
if ((fabsf(_heading_error) < minTurningSpeed) && (headingPGain > 0.01f)) {
_heading_error = (_heading_error > 0) ? minTurningSpeed * 1 / headingPGain : -minTurningSpeed * 1 / headingPGain;
}
break;