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;
}
}
internal void OnPitchDataSampled(int id)
{
if (_mcp.VSHold || _mcp.AltitudeHold)
{
if (!double.IsNaN(Timeline.Data[id].Pitch.Value))
{
var pitchValue = Timeline.LatestAvg(3, f => f.Pitch.Value, id);
var output = _pitchPid.Compute(pitchValue, _mcp.VS, GetTimeBetweenThisFrameAndLastGoodFrame(id, (f) => f.Pitch.Value));
// Trim:
output += Math.Abs(Timeline.RollAvg) * 200;
_control.Set(XINPUT_GAMEPAD_AXIS.LEFT_THUMB_Y, (int)RemoveDeadZone(-1 * output, 7500, 13000));
Timeline.Data[id].Pitch.OutputValue = output;
}
Timeline.Data[id].Pitch.SetpointValue = _mcp.VS;
}
_control.Flush();
}