internal bool UpdateLocation()
{
if (Points.Count == 0)
{
return(false);
}
if (CurrentIndex == Points.Count)
{
return(false);
}
var dist = Math2.GetDistance(Points[CurrentIndex], Timeline.CurrentLocation);
double dist_max = 50;
if (CurrentIndex > 0 && CurrentIndex + 1 < Points.Count - 1)
{
var nextLine = Math2.GetPolarHeadingFromLine(Points[CurrentIndex], Points[CurrentIndex + 1]);
var angle_delta = Math.Abs(Math2.DiffAngles(Math2.GetPolarHeadingFromLine(TargetLine), nextLine));
dist_max += angle_delta * 2.5;
}
if (dist < dist_max)
{
CurrentIndex++;
Trace.WriteLine($"Flight Plan: Advance: {CurrentIndex}");
App.Current.Dispatcher.BeginInvoke((Action)(() =>
{
PropertyChanged?.Invoke(this, new PropertyChangedEventArgs(nameof(CurrentIndex)));
}));
}
return(dist < dist_max);
}
internal void OnCompassDataSampled(int id)
{
if (_mcp.HeadingHold | _mcp.LNAV)
{
var val = Timeline.Data[id].Heading.Value;
if (!double.IsNaN(val) && Timeline.Speed > 10 && Timeline.IsInGame)
{
var diff = Math2.DiffAngles(val, _mcp.HDG);
var aDiff = Math.Abs(diff);
if (aDiff > 1 && id % 4 == 0)
{
//aDiff = Math.Min(aDiff / 3, 100);
aDiff = SystemManager.Instance.Nav.IsOnGlidePath ? aDiff / 3 : 1;
if (diff < 0)
{
_control.Press(XINPUT_GAMEPAD_BUTTONS.RIGHT_SHOULDER, (int)aDiff);
Timeline.Data[id].Heading.OutputValue = -1 * aDiff;
}
else
{
_control.Press(XINPUT_GAMEPAD_BUTTONS.LEFT_SHOULDER, (int)aDiff);
Timeline.Data[id].Heading.OutputValue = aDiff;
}
}
}
Timeline.Data[id].Heading.SetpointValue = _mcp.HDG;
}
}
internal void OnRollDataSampled(int id)
{
if (double.IsNaN(Timeline.Heading))
{
return;
}
if (_mcp.BankHold | _mcp.HeadingHold | _mcp.LNAV)
{
if (!double.IsNaN(Timeline.Data[id].Roll.Value))
{
if (_mcp.HeadingHold | _mcp.LNAV)
{
var rollDelta = Math2.DiffAngles(Timeline.Heading, _mcp.HDG) * 0.7;
var rollMax = 25;
if (Math.Abs(rollDelta) > 5)
{
// if (SystemManager.Instance.Nav.IsOnGlidePath90) rollMax = 0;
var newRoll = Math2.Clamp(-1 * rollDelta, -1 * rollMax, rollMax);
_mcp.Bank += (_mcp.Bank > newRoll) ? -.15 : .15;
if (Timeline.Altitude < 800)
{
_mcp.Bank = 0;
}
}
else
{
_mcp.Bank = 0;
}
}
var rollValue = Timeline.LatestAvg(3, f => f.Roll.Value, id);
var output = _rollPid.Compute(
rollValue,
_mcp.Bank,
GetTimeBetweenThisFrameAndLastGoodFrame(id, (f) => f.Roll.Value));
_control.Set(XINPUT_GAMEPAD_AXIS.LEFT_THUMB_X, (int)RemoveDeadZone(output, 8500, 9500));
Timeline.Data[id].Roll.OutputValue = output;
}
Timeline.Data[id].Roll.SetpointValue = _mcp.Bank;
}
}
