mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 14:48:28 -04:00
263 lines
7.1 KiB
C++
263 lines
7.1 KiB
C++
#include <string.h>
|
|
#include <math.h>
|
|
#include "XPLMDisplay.h"
|
|
#include "XPLMGraphics.h"
|
|
#include "XPLMCamera.h"
|
|
#include "XPLMPlanes.h"
|
|
#include "XPLMUtilities.h"
|
|
#include "XPLMDataAccess.h"
|
|
#if LIN
|
|
#include <GL/gl.h>
|
|
#else
|
|
#if __GNUC__
|
|
#include <OpenGL/gl.h>
|
|
#else
|
|
#include <windows.h>
|
|
#include <..\..\..\..\..\..\..\program files (x86)\microsoft sdks\windows\v7.0a\include\gl\gl.h>
|
|
#endif
|
|
#endif
|
|
#include "AircraftUtils.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
using namespace System;
|
|
using namespace System::Net;
|
|
using namespace System::Net::Sockets;
|
|
|
|
public ref class Globals abstract sealed {
|
|
public:
|
|
static IPEndPoint^ RemoteIpEndPoint = gcnew IPEndPoint( IPAddress::Any, 4900 );
|
|
static Socket^ XplanesSEND = gcnew Socket(AddressFamily::InterNetwork,
|
|
SocketType::Dgram, ProtocolType::Udp);
|
|
};
|
|
|
|
XPLMDataRef gPlaneX = NULL;
|
|
XPLMDataRef gPlaneY = NULL;
|
|
XPLMDataRef gPlaneZ = NULL;
|
|
XPLMDataRef gPlaneTheta = NULL;
|
|
XPLMDataRef gPlanePhi = NULL;
|
|
XPLMDataRef gPlanePsi = NULL;
|
|
XPLMDataRef gOverRidePlanePosition = NULL;
|
|
XPLMDataRef gAGL = NULL;
|
|
XPLMDataRef gLatRef = NULL;
|
|
XPLMDataRef gLongRef = NULL;
|
|
|
|
float fTextColour[3];
|
|
char szString[100];
|
|
|
|
const double kMaxPlaneDistance = 5280.0 / 3.2 * 10.0;
|
|
const double kFullPlaneDist = 5280.0 / 3.2 * 3.0;
|
|
|
|
static inline float sqr(float a) { return a * a; }
|
|
|
|
static inline float CalcDist3D(float x1, float y1, float z1, float x2, float y2, float z2)
|
|
{
|
|
return sqrt(sqr(x2-x1) + sqr(y2-y1) + sqr(z2-z1));
|
|
}
|
|
|
|
const double kFtToMeters = 0.3048;
|
|
|
|
static XPLMDataRef gFOVDataRef = NULL;
|
|
|
|
int AircraftDrawCallback( XPLMDrawingPhase inPhase,
|
|
int inIsBefore,
|
|
void * inRefcon);
|
|
|
|
PLUGIN_API int XPluginStart( char * outName,
|
|
char * outSig,
|
|
char * outDesc)
|
|
{
|
|
int planeCount;
|
|
XPLMCountAircraft(&planeCount, 0, 0);
|
|
|
|
fTextColour[0] = 1.0;
|
|
fTextColour[1] = 1.0;
|
|
fTextColour[2] = 1.0;
|
|
|
|
|
|
strcpy(outName, "Quad Hil");
|
|
strcpy(outSig, "based on xplanesdk.examples.drawaircraft");
|
|
strcpy(outDesc, "A plugin that receives raw data to display");
|
|
|
|
/* Prefetch the sim variables we will use. */
|
|
gPlaneX = XPLMFindDataRef("sim/flightmodel/position/local_x");
|
|
gPlaneY = XPLMFindDataRef("sim/flightmodel/position/local_y");
|
|
gPlaneZ = XPLMFindDataRef("sim/flightmodel/position/local_z");
|
|
gPlaneTheta = XPLMFindDataRef("sim/flightmodel/position/theta");
|
|
gPlanePhi = XPLMFindDataRef("sim/flightmodel/position/phi");
|
|
gPlanePsi = XPLMFindDataRef("sim/flightmodel/position/psi");
|
|
gOverRidePlanePosition = XPLMFindDataRef("sim/operation/override/override_planepath");
|
|
gAGL = XPLMFindDataRef("sim/flightmodel/position/y_agl");
|
|
gLatRef = XPLMFindDataRef("sim/flightmodel/position/lat_ref");
|
|
gLongRef = XPLMFindDataRef("sim/flightmodel/position/lon_ref");
|
|
|
|
|
|
/* Next register the drawing callback. We want to be drawn
|
|
* after X-Plane draws its 3-d objects. */
|
|
XPLMRegisterDrawCallback(
|
|
AircraftDrawCallback,
|
|
xplm_Phase_Window, /* Draw when sim is doing objects */
|
|
0, /* After objects */
|
|
NULL); /* No refcon needed */
|
|
|
|
Globals::XplanesSEND->Bind(Globals::RemoteIpEndPoint);
|
|
/*
|
|
char FileName[256], AircraftPath[256];
|
|
|
|
for (long index = 0; index < planeCount; ++index)
|
|
{
|
|
XPLMGetNthAircraftModel(index, FileName, AircraftPath);
|
|
pAircraft[index] = (char *)AircraftPath;
|
|
|
|
if (XPLMAcquirePlanes((char **)&pAircraft, NULL, NULL))
|
|
XPLMSetAircraftModel(index, AircraftPath);
|
|
}
|
|
*/
|
|
|
|
return 1;
|
|
}
|
|
|
|
|
|
PLUGIN_API void XPluginStop(void)
|
|
{
|
|
}
|
|
|
|
PLUGIN_API void XPluginDisable(void)
|
|
{
|
|
int Enable[10];
|
|
Enable[0] = 0;
|
|
|
|
XPLMSetDatavi(gOverRidePlanePosition,&Enable[0],0,1); // disable physics
|
|
}
|
|
|
|
PLUGIN_API int XPluginEnable(void)
|
|
{
|
|
int Enable[10];
|
|
Enable[0] = 1;
|
|
|
|
XPLMSetDatavi(gOverRidePlanePosition,&Enable[0],0,1); // disable physics
|
|
|
|
return 1;
|
|
}
|
|
|
|
PLUGIN_API void XPluginReceiveMessage( XPLMPluginID inFromWho,
|
|
long inMessage,
|
|
void * inParam)
|
|
{
|
|
}
|
|
|
|
|
|
/*
|
|
* AircraftDrawCallback
|
|
*
|
|
* This is the actual drawing callback that does the work; for us it will
|
|
* be called after X-Plane has drawn its 3-d objects. The coordinate system
|
|
* is 'normal' for 3-d drawing, meaning 0,0,0 is at the earth's surface at the
|
|
* lat/lon reference point, with +Y = up, +Z = South, +X = East. (Note that
|
|
* these relationships are only true at 0,0,0 due to the Earth's curvature!!)
|
|
*
|
|
* Drawing hooks that draw before X-Plane return 1 to let X-Plane draw or 0
|
|
* to inhibit drawing. For drawing hooks that run after X-Plane, this return
|
|
* value is ignored but we will return 1 anyway.
|
|
*
|
|
*/
|
|
|
|
public struct quad
|
|
{
|
|
double t1;
|
|
double t2;
|
|
double t3;
|
|
double t4;
|
|
|
|
double lat;
|
|
double lng;
|
|
double alt;
|
|
double altagl;
|
|
|
|
double roll;
|
|
double pitch;
|
|
double yaw;
|
|
};
|
|
|
|
int AircraftDrawCallback( XPLMDrawingPhase inPhase,
|
|
int inIsBefore,
|
|
void * inRefcon)
|
|
{
|
|
if (Globals::XplanesSEND->Available > 0) {
|
|
|
|
array<Byte>^bytesReceived = gcnew array<Byte>(256);
|
|
int bytes = 0;
|
|
bytes = Globals::XplanesSEND->Receive( bytesReceived, bytesReceived->Length, static_cast<SocketFlags>(0) );
|
|
|
|
int count = 0;
|
|
double t1 = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double t2 = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double t3 = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double t4 = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
|
|
count++;
|
|
double lat = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double lng = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double alt = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double altagl = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
|
|
count++;
|
|
double roll = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double pitch = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
count++;
|
|
double yaw = BitConverter::ToDouble(bytesReceived, count * 8);
|
|
|
|
double X,Y,Z;
|
|
|
|
XPLMSetDataf(gLatRef,(float)lat);
|
|
XPLMSetDataf(gLongRef,(float)lng);
|
|
|
|
|
|
XPLMWorldToLocal(lat,lng,alt * kFtToMeters,&X,&Y,&Z);
|
|
|
|
XPLMSetDataf(gPlaneX, X);
|
|
XPLMSetDataf(gPlaneY, Y);
|
|
XPLMSetDataf(gPlaneZ, Z);
|
|
|
|
|
|
XPLMSetDataf(gPlaneTheta, pitch);
|
|
XPLMSetDataf(gPlanePhi, roll);
|
|
XPLMSetDataf(gPlanePsi, yaw);
|
|
|
|
sprintf(szString,"m1: %f, m2: %f, m3: %f, m4: %f\0", t1,t2,t3,t4);
|
|
XPLMDrawString(fTextColour,700,700,szString,NULL,xplmFont_Basic);
|
|
|
|
|
|
return 1;
|
|
|
|
} else {
|
|
return 1;
|
|
}
|
|
|
|
|
|
double x,y,z,theta,phi,psi;
|
|
|
|
//double Lat = 34.09, Lon = -117.25, Alt = 1170;
|
|
float Heading = 0, Pitch = 0, Roll = 0, Altitude;
|
|
|
|
x = XPLMGetDataf(gPlaneX);
|
|
y = XPLMGetDataf(gPlaneY);
|
|
z = XPLMGetDataf(gPlaneZ);
|
|
theta = XPLMGetDataf(gPlaneTheta);
|
|
phi = XPLMGetDataf(gPlanePhi);
|
|
psi = XPLMGetDataf(gPlanePsi);
|
|
Altitude = XPLMGetDataf(gAGL);
|
|
|
|
|
|
XPLMSetDataf(gPlaneX, x + 1);
|
|
|
|
return 1;
|
|
}
|