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