public void StabilizeDirection()
{
isStabilizing = true;
CancellationToken token = stabilizeTokenSource.Token;
stabilizeTargetDirection = positionSensor.ReadHeading();
float previousHeading = stabilizeTargetDirection;
float currentHeading = stabilizeTargetDirection;
float deltaAngle = 0;
int direction;
float turnRate;
stabilizePid.CalculateControlOutput(); //to reset last update time
stabilizePid.ResetIntegrator();
Thread stabilizeThread = new Thread(() =>
{
while (true)
{
if (token.IsCancellationRequested)
{
break;
}
currentHeading = positionSensor.ReadHeading();
deltaAngle = currentHeading - previousHeading;
if (Math.Abs(deltaAngle) > 180) //If angle is ok we only check once, if it's abnormal then we can spend more time as this will happen rarely
{
if (deltaAngle > 180)
{
deltaAngle -= 360;
}
if (deltaAngle < 180)
{
deltaAngle += 360;
}
}
direction = Math.Sign(deltaAngle);
turnPid.ActualInput = currentHeading;
turnRate = turnPid.CalculateControlOutput();
SetTurn((int)Math.Round(turnRate));
previousHeading = currentHeading;
Thread.Sleep(stabilizeTimeDelta);
}
});
stabilizeThread.Start();
}
