internal void OnAltidudeDataSampled(int id)
{
if (_mcp.AltitudeHold)
{
var altitudeDelta = _mcp.ALT - Timeline.AltitudeAvg;
var mult = 10;
if (SystemManager.Instance.Nav.IsOnGlidePath)
{
mult = 6;
}
_mcp.VS = Math2.Clamp(altitudeDelta / mult, -15, 20);
Timeline.Data[id].Altitude.SetpointValue = _mcp.ALT;
}
}
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;
}
}
