409 lines
18 KiB
C#
409 lines
18 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 const double EarthRadiusInFeet = 20902230;
|
|
|
|
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 double lastCalcDispTrue = 0;
|
|
|
|
private bool paused;
|
|
|
|
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
|
|
struct Struct1
|
|
{
|
|
public double nav1freq;
|
|
public double nav1crs;
|
|
public double nav1cdi;
|
|
public double gspeed;
|
|
public double velWorldX;
|
|
public double velWorldZ;
|
|
public double magVar;
|
|
|
|
public SIMCONNECT_DATA_LATLONALT navPos;
|
|
public SIMCONNECT_DATA_LATLONALT pos;
|
|
public int code;
|
|
public double navMagVar;
|
|
public double locCrs;
|
|
};
|
|
|
|
enum DEFINITIONS
|
|
{
|
|
Struct1,
|
|
}
|
|
|
|
enum REQUESTS
|
|
{
|
|
Struct1,
|
|
VOR,
|
|
}
|
|
|
|
enum EVENTS
|
|
{
|
|
Pause,
|
|
AP_NAV1_HOLD_ON,
|
|
AP_NAV_SELECT_SET,
|
|
}
|
|
|
|
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;
|
|
|
|
NAV1Freq.Content = "";
|
|
NAV1Crs.Content = "";
|
|
NAV1Dev.Content = "";
|
|
GSPD.Content = "";
|
|
TRK.Content = "";
|
|
DeltaT.Content = "";
|
|
BlueValues.Text = "";
|
|
DispRate.Content = "";
|
|
Disp.Content = "";
|
|
Capture.Content = "";
|
|
DeltaDev.Content = "";
|
|
TimeToCenter.Content = "";
|
|
BlueValues.Text = "";
|
|
BlueValuesFake.Text = "";
|
|
ExpValues.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.OnRecvVorList += OnRecvVorList;
|
|
|
|
SimConnect.SubscribeToSystemEvent(EVENTS.Pause, "PAUSE");
|
|
|
|
SimConnect.MapClientEventToSimEvent(EVENTS.AP_NAV1_HOLD_ON, "AP_NAV1_HOLD_ON");
|
|
SimConnect.MapClientEventToSimEvent(EVENTS.AP_NAV_SELECT_SET, "AP_NAV_SELECT_SET");
|
|
SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.AP_NAV1_HOLD_ON, false);
|
|
SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.AP_NAV_SELECT_SET, false);
|
|
SimConnect.SetNotificationGroupPriority(GROUPS.Highest, SimConnect.SIMCONNECT_GROUP_PRIORITY_HIGHEST);
|
|
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV ACTIVE FREQUENCY:1", "MHz", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
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);
|
|
|
|
// for just sim based data
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV VOR LATLONALT:1", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "STRUCT LATLONALT", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV CODES:1", null, SIMCONNECT_DATATYPE.INT32, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV MAGVAR:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV LOCALIZER:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
// for navigraph
|
|
// SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "STRUCT LATLONALT", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);
|
|
|
|
SimConnect.RegisterDataDefineStruct<Struct1>(DEFINITIONS.Struct1);
|
|
|
|
// Calculation using actual sim deviation only works if set to once every second
|
|
SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SIM_FRAME, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
|
|
//SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SECOND, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
|
|
stopWatch.Start();
|
|
|
|
SimConnect.SubscribeToFacilities(SIMCONNECT_FACILITY_LIST_TYPE.VOR, REQUESTS.VOR);
|
|
|
|
buttonConnection.Content = "Disconnect";
|
|
}
|
|
catch (COMException ex)
|
|
{
|
|
// A connection to the SimConnect server could not be established
|
|
SimConnect = null;
|
|
|
|
MessageBox.Show(ex.ToString());
|
|
}
|
|
}
|
|
|
|
private void OnRecvVorList(SimConnect sender, SIMCONNECT_RECV_VOR_LIST data)
|
|
{
|
|
switch ((REQUESTS)data.dwRequestID)
|
|
{
|
|
case REQUESTS.VOR:
|
|
{
|
|
foreach (SIMCONNECT_DATA_FACILITY_VOR vor in data.rgData)
|
|
{
|
|
var v = vor;
|
|
}
|
|
break;
|
|
}
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
/// <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:
|
|
{
|
|
double deltaT = stopWatch.ElapsedMilliseconds;
|
|
stopWatch.Restart();
|
|
|
|
if (paused) return;
|
|
|
|
Struct1 d = (Struct1)data.dwData[0];
|
|
|
|
// Actual:
|
|
double dev = Math.Abs(d.nav1cdi) * (155.0 / 127.0);
|
|
double _trk = ToDegrees(Math.Atan2(d.velWorldX, d.velWorldZ));
|
|
double trk = (_trk < 0 ? _trk + 360 : _trk) - d.magVar;
|
|
double deltaTrk = d.nav1crs - trk;
|
|
|
|
//Faked:
|
|
double distToLoc = CalculateDistance(d.pos.Latitude, d.pos.Longitude, d.navPos.Latitude, d.navPos.Longitude);
|
|
double magVar = d.navMagVar > 180 ? d.navMagVar - 360 : d.navMagVar;
|
|
double brgToLoc = CalculateBearing(d.pos.Latitude, d.pos.Longitude, d.navPos.Latitude, d.navPos.Longitude);
|
|
double calcDisp = distToLoc * Math.Sin(ToRadians(Math.Abs(d.nav1crs - (brgToLoc + magVar))));
|
|
double calcDispTrue = distToLoc * Math.Sin(ToRadians(Math.Abs(d.locCrs - brgToLoc)));
|
|
|
|
if (lastFrame.HasValue)
|
|
{
|
|
// Faked cont.
|
|
double calcDispRate = ((calcDispTrue - lastCalcDispTrue) < 0 ? -1 : 1) * d.gspeed * Math.Sin(ToRadians(Math.Abs(d.locCrs - trk)));
|
|
|
|
ExpValues.Text = $"Dist to LOC: {distToLoc} ft\n" +
|
|
$"True BRG to LOC: {brgToLoc}°\n" +
|
|
$"Calced Disp Rate: {calcDispRate} ft/s\n" +
|
|
$"TRU LOC CRS: {d.locCrs}°\n" +
|
|
$"Calced Disp User CRS: {calcDisp} ft\n" +
|
|
$"Calced Disp True CRS: {calcDispTrue} ft\n";
|
|
|
|
// Actual cont.
|
|
double lastDev = Math.Abs(lastFrame.Value.nav1cdi) * (155.0 / 127.0);
|
|
double deltaDev = (dev - lastDev) * (1000 / deltaT);
|
|
// Requires a deviation rate != 0 to calculate properly
|
|
double timeToCenter = Math.Abs(dev / deltaDev);
|
|
double dispRate = (deltaDev < 0 ? -1 : 1) * d.gspeed * Math.Sin(ToRadians(Math.Abs(deltaTrk)));
|
|
// Requires dispRate > 0 to calculate properlys
|
|
double disp = timeToCenter * Math.Abs(dispRate);
|
|
|
|
bool blueCaptureOG = BlueCheck(deltaTrk, dispRate, disp);
|
|
bool redCaptureOG = RedCheck(dispRate, disp);
|
|
|
|
bool blueCaptureFK = BlueCheck(deltaTrk, calcDispRate, calcDispTrue, true);
|
|
bool redCaptureFK = RedCheck(calcDispRate, calcDispTrue);
|
|
|
|
// Check for Full course, Half course and Quarter course nominal widths at ILS reference point
|
|
bool dispValid = !(double.IsInfinity(disp) ||
|
|
double.IsNaN(disp) ||
|
|
(dev >= 155 && disp < 350) ||
|
|
(dev >= 77.5 && disp < 175) ||
|
|
(dev >= 38.75 && disp < 87.5));
|
|
|
|
if (deltaDev != 0)
|
|
{
|
|
DispRate.Content = $"Disp. rate: {calcDispRate} ft/s";
|
|
Disp.Content = $"Disp.: {calcDispTrue} ft, VALID OG: {dispValid}";
|
|
Capture.Content = $"Capture criteria: BLUE OG: {blueCaptureOG}, BLUE FK: {blueCaptureFK}, RED OG: {redCaptureOG}, RED FK: {redCaptureFK}";
|
|
DeltaDev.Content = $"Deltas: Dev: {deltaDev} DDM/s, TRK: {deltaTrk}";
|
|
TimeToCenter.Content = $"Time to Center: {timeToCenter} s";
|
|
}
|
|
|
|
// Use faked values cause they are more accurate
|
|
if (blueCaptureFK || redCaptureFK)
|
|
{
|
|
SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.AP_NAV_SELECT_SET, 1, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
|
|
SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.AP_NAV1_HOLD_ON, 0, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
|
|
}
|
|
|
|
}
|
|
lastFrame = d;
|
|
lastCalcDispTrue = calcDispTrue;
|
|
|
|
NAV1Freq.Content = $"NAV1 Freq: {d.nav1freq} MHz";
|
|
NAV1Crs.Content = $"NAV1 Crs: {d.nav1crs}°";
|
|
NAV1Dev.Content = $"NAV1 DDM: {dev} DDM";
|
|
GSPD.Content = $"GRND SPD: {d.gspeed} ft/s";
|
|
TRK.Content = $"TRK MAG: {trk}°";
|
|
DeltaT.Content = $"Delta t: {deltaT} ms";
|
|
|
|
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 Capture Criteria
|
|
|
|
private bool BlueCheck(double deltaTrk, double dispRate, double disp, bool fake = false)
|
|
{
|
|
double normDeltaTrack = (Math.Abs(Math.Max(Math.Min(deltaTrk, 90), -90)) * 0.2) / 90;
|
|
double gainedNormDeltaTrack = normDeltaTrack + (Gain.Value * 0.8);
|
|
double dispRateMulGainedNormDeltaTrack = gainedNormDeltaTrack * dispRate;
|
|
double rescaledDisp = disp * 0.065;
|
|
double preValue = rescaledDisp + dispRateMulGainedNormDeltaTrack;
|
|
double value = preValue * disp;
|
|
|
|
if (fake)
|
|
{
|
|
BlueValuesFake.Text = $"Norm Delta TRK: {normDeltaTrack}\n" +
|
|
$"Gained Norm Delta TRK: {gainedNormDeltaTrack}\n" +
|
|
$"Disp rate Mul Gained Norm Delta TRK: {dispRateMulGainedNormDeltaTrack}\n" +
|
|
$"Rescaled Displacement: {rescaledDisp}\n" +
|
|
$"Pre value: {preValue}\n" +
|
|
$"Value: {value}";
|
|
}
|
|
else
|
|
{
|
|
BlueValues.Text = $"Norm Delta TRK: {normDeltaTrack}\n" +
|
|
$"Gained Norm Delta TRK: {gainedNormDeltaTrack}\n" +
|
|
$"Disp rate Mul Gained Norm Delta TRK: {dispRateMulGainedNormDeltaTrack}\n" +
|
|
$"Rescaled Displacement: {rescaledDisp}\n" +
|
|
$"Pre value: {preValue}\n" +
|
|
$"Value: {value}";
|
|
}
|
|
return value < 0;
|
|
}
|
|
|
|
private bool RedCheck(double dispRate, double disp)
|
|
{
|
|
return Math.Abs(dispRate) < 25 && disp < 500;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Helpers
|
|
|
|
private double CalculateDistance(double lat1, double lon1, double lat2, double lon2)
|
|
{
|
|
double dLat = ToRadians(lat2 - lat1);
|
|
double dLon = ToRadians(lon2 - lon1);
|
|
double lat1Rad = ToRadians(lat1);
|
|
double lat2Rad = ToRadians(lat2);
|
|
double sinDLat = Math.Sin(dLat / 2);
|
|
double sinDLon = Math.Sin(dLon / 2);
|
|
|
|
double a = sinDLat * sinDLat + sinDLon * sinDLon * Math.Cos(lat1Rad) * Math.Cos(lat2Rad);
|
|
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
|
|
|
|
return EarthRadiusInFeet * c;
|
|
}
|
|
|
|
private double CalculateBearing(double lat1, double lon1, double lat2, double lon2)
|
|
{
|
|
double dLon = ToRadians(lon2 - lon1);
|
|
double lat1Rad = ToRadians(lat1);
|
|
double lat2Rad = ToRadians(lat2);
|
|
|
|
double y = Math.Sin(dLon) * Math.Cos(lat2Rad);
|
|
double x = Math.Cos(lat1Rad) * Math.Sin(lat2Rad) - Math.Sin(lat1Rad) * Math.Cos(lat2Rad) * Math.Cos(dLon);
|
|
double bearing = Math.Atan2(y, x);
|
|
|
|
// Convert bearing from radians to degrees
|
|
bearing *= (180 / Math.PI);
|
|
|
|
// Normalize bearing to range from 0 to 360 degrees
|
|
return (bearing + 360) % 360;
|
|
}
|
|
|
|
private double ToRadians(double degrees) => (Math.PI / 180) * degrees;
|
|
|
|
private double ToDegrees(double radians) => (180/ Math.PI) * radians;
|
|
|
|
#endregion
|
|
}
|
|
}
|