forked from Archive/PX4-Autopilot
Differential Drive: Improved logic, to be independent of the heading gain
This commit is contained in:
parent
d0aab458e5
commit
83d91c1489
|
@ -86,10 +86,11 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
|
||||||
}
|
}
|
||||||
|
|
||||||
float minTurningSpeed = _param_rdd_min_turning_speed.get();
|
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
|
// Make sure we do not get stuck while turning as the error gets too small
|
||||||
if (fabsf(_heading_error) < minTurningSpeed) {
|
if ((fabsf(_heading_error) < minTurningSpeed) && (headingPGain > 0.01f)) {
|
||||||
_heading_error = (_heading_error > 0) ? minTurningSpeed : -minTurningSpeed;
|
_heading_error = (_heading_error > 0) ? minTurningSpeed * 1 / headingPGain : -minTurningSpeed * 1 / headingPGain;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue