305 lines
11 KiB
C#
305 lines
11 KiB
C#
using Microsoft.FlightSimulator.SimConnect;
|
|
using System;
|
|
using System.Diagnostics;
|
|
using System.Runtime.InteropServices;
|
|
using System.Windows;
|
|
using System.Windows.Interop;
|
|
|
|
namespace MD11_Localizer_Capture
|
|
{
|
|
/// <summary>
|
|
/// Interaction logic for MainWindow.xaml
|
|
/// </summary>
|
|
public partial class MainWindow : Window
|
|
{
|
|
// User-defined win32 event
|
|
private const int WM_USER_SIMCONNECT = 0x0402;
|
|
|
|
private readonly IntPtr Handle;
|
|
private readonly HwndSource HandleSource;
|
|
|
|
// Declare a SimConnect object
|
|
private SimConnect SimConnect = null;
|
|
|
|
private readonly Stopwatch stopWatch = new();
|
|
|
|
private Struct1? lastFrame = null;
|
|
|
|
private bool paused;
|
|
|
|
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
|
|
struct Struct1
|
|
{
|
|
public double nav1crs;
|
|
public double nav1cdi;
|
|
public double gspeed;
|
|
public double velWorldX;
|
|
public double velWorldZ;
|
|
public double magVar;
|
|
};
|
|
|
|
enum DEFINITIONS
|
|
{
|
|
Struct1,
|
|
}
|
|
|
|
enum REQUESTS
|
|
{
|
|
Struct1,
|
|
VOR,
|
|
}
|
|
|
|
enum EVENTS
|
|
{
|
|
Pause,
|
|
TglPause,
|
|
}
|
|
|
|
enum GROUPS
|
|
{
|
|
Highest = 1,
|
|
}
|
|
|
|
public MainWindow()
|
|
{
|
|
InitializeComponent();
|
|
|
|
Handle = new WindowInteropHelper(this).EnsureHandle(); // Get handle of main WPF Window
|
|
HandleSource = HwndSource.FromHwnd(Handle); // Get source of handle in order to add event handlers to it
|
|
HandleSource.AddHook(HandleSimConnectEvents);
|
|
}
|
|
|
|
~MainWindow()
|
|
{
|
|
HandleSource?.RemoveHook(HandleSimConnectEvents);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Connects / Disconnect from FS
|
|
/// </summary>
|
|
/// <param name="sender">Sender object</param>
|
|
/// <param name="e">Event arguments</param>
|
|
private void ButtonConnection_Click(object sender, RoutedEventArgs e)
|
|
{
|
|
if (SimConnect != null)
|
|
{
|
|
SimConnect.Dispose();
|
|
SimConnect = null;
|
|
lastFrame = null;
|
|
|
|
BlueValues.Text = "";
|
|
|
|
buttonConnection.Content = "Connect";
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
SimConnect = new SimConnect("MD11 Localizer Capture", Handle, WM_USER_SIMCONNECT, null, 0);
|
|
|
|
SimConnect.OnRecvSimobjectData += OnRecvSimobjectData;
|
|
SimConnect.OnRecvEvent += OnRecvEvent;
|
|
|
|
SimConnect.SubscribeToSystemEvent(EVENTS.Pause, "PAUSE");
|
|
|
|
SimConnect.MapClientEventToSimEvent(EVENTS.TglPause, "KEY_PAUSE_ON ");
|
|
SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.TglPause, false);
|
|
SimConnect.SetNotificationGroupPriority(GROUPS.Highest, SimConnect.SIMCONNECT_GROUP_PRIORITY_HIGHEST);
|
|
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV OBS:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV CDI:1", "Number", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "GROUND VELOCITY", "Feet/Second", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "VELOCITY WORLD X", "Radians", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "VELOCITY WORLD Z", "Radians", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "MAGVAR", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
|
|
SimConnect.RegisterDataDefineStruct<Struct1>(DEFINITIONS.Struct1);
|
|
|
|
SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SIM_FRAME, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
|
|
stopWatch.Start();
|
|
|
|
buttonConnection.Content = "Disconnect";
|
|
}
|
|
catch (COMException ex)
|
|
{
|
|
// A connection to the SimConnect server could not be established
|
|
SimConnect = null;
|
|
|
|
MessageBox.Show(ex.ToString());
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Win32 Message Pump equivalent
|
|
/// </summary>
|
|
/// <param name="hWnd">Window Handel</param>
|
|
/// <param name="message">Message ID</param>
|
|
/// <param name="wParam">Parameters</param>
|
|
/// <param name="lParam">Parameters</param>
|
|
/// <param name="isHandled">WFlag indicating application handeled messag or not</param>
|
|
/// <returns></returns>
|
|
private IntPtr HandleSimConnectEvents(IntPtr hWnd, int message, IntPtr wParam, IntPtr lParam, ref bool isHandled)
|
|
{
|
|
isHandled = false;
|
|
|
|
switch (message)
|
|
{
|
|
case WM_USER_SIMCONNECT:
|
|
{
|
|
if (SimConnect != null)
|
|
{
|
|
SimConnect.ReceiveMessage();
|
|
isHandled = true;
|
|
}
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return IntPtr.Zero;
|
|
}
|
|
|
|
private void OnRecvSimobjectData(SimConnect sender, SIMCONNECT_RECV_SIMOBJECT_DATA data)
|
|
{
|
|
switch ((REQUESTS)data.dwRequestID)
|
|
{
|
|
case REQUESTS.Struct1:
|
|
{
|
|
if (paused) return;
|
|
|
|
Struct1 d = (Struct1)data.dwData[0];
|
|
|
|
if (lastFrame.HasValue && lastFrame.Value.nav1cdi != d.nav1cdi)
|
|
{
|
|
double deltaT = stopWatch.ElapsedMilliseconds;
|
|
stopWatch.Restart();
|
|
|
|
double displacement = FCC_GetDisplacement(d, deltaT);
|
|
double dispRate = FCC_GetDisplacementRate(d, deltaT);
|
|
|
|
if (dispRate != 0.0)
|
|
{
|
|
bool blueBox;
|
|
{
|
|
double deltaTrack = Math.Abs(d.nav1crs - IRU_GetMagTrack(d));
|
|
deltaTrack = Math.Max(Math.Min(deltaTrack, 90), -90);
|
|
deltaTrack = Math.Abs(deltaTrack) * 0.2 / 90.0;
|
|
double GAIN = 1.0; //tune this?
|
|
deltaTrack += 0.8 * GAIN;
|
|
deltaTrack *= dispRate;
|
|
|
|
double scaledDisp = displacement * 0.065;
|
|
|
|
double blue = scaledDisp + deltaTrack;
|
|
blue *= displacement;
|
|
|
|
blueBox = blue < 0;
|
|
}
|
|
|
|
bool redBox = Math.Abs(displacement) < 500 && Math.Abs(dispRate) < 25;
|
|
|
|
BlueValues.Text = $"Displacement: {displacement}\n" +
|
|
$"Displacement Rate: {dispRate}\n" +
|
|
$"BlueBox: {blueBox}\n" +
|
|
$"RedBox: {redBox}\n" +
|
|
$"Delta T: {deltaT}\n" +
|
|
$"Deviation Rate: {FCC_GetDeviationRate(d, deltaT)}";
|
|
|
|
if (redBox || blueBox)
|
|
{
|
|
SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.TglPause, 1, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
|
|
}
|
|
}
|
|
|
|
}
|
|
lastFrame = d;
|
|
|
|
break;
|
|
}
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
private void OnRecvEvent(SimConnect sender, SIMCONNECT_RECV_EVENT data)
|
|
{
|
|
switch ((EVENTS)data.uEventID)
|
|
{
|
|
case EVENTS.Pause:
|
|
{
|
|
paused = data.dwData == 1;
|
|
if (data.dwData == 0) stopWatch.Start();
|
|
else stopWatch.Stop();
|
|
break;
|
|
}
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
#region Collin Funcs
|
|
|
|
// This just returns *magentic* track
|
|
double IRU_GetMagTrack(Struct1 data) {
|
|
double trk = ToDegrees(Math.Atan2(data.velWorldX, data.velWorldZ));
|
|
return (trk < 0 ? trk + 360 : trk) - data.magVar;
|
|
}
|
|
|
|
double FCC_GetDisplacement(Struct1 data, double lastCdiTime)
|
|
{
|
|
double rate = FCC_GetDeviationRate (data, lastCdiTime);
|
|
if (rate != 0)
|
|
{
|
|
// Move division into Abs
|
|
double distToIntercept = Math.Abs(data.nav1cdi / rate);
|
|
// My GSpeed is in ft/s, dunno if that migh cause issues (shouldn't but eh)
|
|
distToIntercept *= data.gspeed;
|
|
|
|
double deltaTrack = Math.Abs(data.nav1crs - IRU_GetMagTrack(data));
|
|
double displacement = distToIntercept * Math.Sin(ToRadians(deltaTrack));
|
|
|
|
// Return Absolute value
|
|
return Math.Abs(displacement);
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
double FCC_GetDisplacementRate(Struct1 data, double lastCdiTime)
|
|
{
|
|
if (lastFrame.HasValue && lastFrame.Value.nav1cdi != data.nav1cdi && lastCdiTime != 0)
|
|
{
|
|
// Switch subtraction
|
|
// Otherwise, at least for me, the sign of the output was inverted (- for drifitng away instead of +)
|
|
double deltaDev = lastFrame.Value.nav1cdi - data.nav1cdi;
|
|
double deltaTrack = Math.Abs(data.nav1crs - IRU_GetMagTrack(data));
|
|
double dispRate = (deltaDev < 0 ? -1 : 1) * data.gspeed * Math.Abs(Math.Sin(ToRadians(deltaTrack)));
|
|
|
|
return dispRate;
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
double FCC_GetDeviationRate(Struct1 data, double lastCdiTime)
|
|
{
|
|
if (lastFrame.HasValue && lastFrame.Value.nav1cdi != data.nav1cdi && lastCdiTime != 0)
|
|
{
|
|
// Switch subtraction
|
|
// Adjust for ms to s by multiplying by th time step expressed as seconds
|
|
double devRate = (lastFrame.Value.nav1cdi - data.nav1cdi) * (1000 / lastCdiTime);
|
|
return Math.Abs(devRate);
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Helpers
|
|
|
|
private double ToRadians(double degrees) => (Math.PI / 180) * degrees;
|
|
|
|
private double ToDegrees(double radians) => (180/ Math.PI) * radians;
|
|
|
|
#endregion
|
|
}
|
|
}
|